perm filename INTERP.PAL[AL,HE]36 blob sn#521555 filedate 1980-07-10 generic text, type C, neo UTF8
COMMENT ⊗   VALID 00039 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00005 00002	.SBTTL Interpreter	Data structures
C00017 00003	INTINIT, NEWENV, MINTS, MRKENV, MRKHDR
C00024 00004	Interpreter itself: INTERP
C00028 00005	  GETARG, GETENV, GETSCA, GETVEC, GETTRN
C00035 00006	Variable declaration:  MVAR, KVAR
C00047 00007	Stack ops: GTVAL, CHNGE, PUSHV, POPV
C00051 00008	Flow-of-control: PROC, RETURN
C00061 00009	  ABORT, GODDT, NOOP, FORCHK, FOREND, CASE, JUMP, JUMPC
C00068 00010	  SPAWN, SPROUT, TERMINATE
C00076 00011	Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT,LXOR,EQV
C00082 00012	return scalars: SABS,SADD,SSUB,SMUL,SDIV,SNEG,SEXP,MAX,MIN,INT,IDIV,MOD
C00088 00013			 VDOT, VMAG, SSBRTN
C00093 00014	Vector utilities:  UNITV, CROSV
C00099 00015	TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN
C00107 00016	Return vectors: SVMUL, VSDIV, TVMUL, VMAKE, VADD, VSUB
C00114 00017	Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR, CONSTR
C00128 00018	Affixment: AFFIX
C00137 00019		   UNFIX
C00143 00020	Motion:  MOVE (start: code for finger motions)
C00151 00021		 MOVE (continued: setup for arm motion)
C00168 00022		 MOVE (continued: speed-factor & time constraints)
C00179 00023		 MOVE (concluded: handle deproach polys, call spline routine & end cleanup)
C00186 00024		 MOVE (auxillary routine: CUBSPL)
C00194 00025		 MOVE (auxillary routines: MOVARG, MOVPOS & move data)
C00202 00026		 CENTER, UPDEPR, OPERATE, STOP
C00208 00027	Common code for motions: MOVSTA & OPMOV
C00217 00028	Error recovery for motions: RETRY, FINISH, PARK
C00222 00029	Force system routines: SETBAS, WRIST, STIFF, GATHR
C00227 00030	Motion auxilary functions: TABOFS, WHERE, NOTICE, GETARM, GETMEC
C00233 00031	Condition monitors:  CMMAK 
C00242 00032	  CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMWAIT, CMUNCR
C00252 00033	  CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST
C00261 00034	Events:  SIGNAL, WAITE, PAUSE
C00265 00035	Input routines:  PROMPT, QUERY, SCALRD
C00268 00036	Output routines:  PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX
C00274 00037	  BREAK, NOOP, TOPAL
C00276 00038	Initialization ops:  PROG, ENDP, FIXIT
C00281 00039	BUGS
C00282 ENDMK
C⊗;
.SBTTL Interpreter	;Data structures

COMMENT ⊗
Register uses in the interpreter:
	R5	used by some routines as the display register
 	R4	points to interpreter status block
 	R3	interpreter stack pointer
 	R2	not used by the main interpreter loop.  Can be munged by
                    any primary interpreter routine.

Each interpreter has a stack which it uses to store pointers to
currently "open" variables.  During the course of a calculation,
operands and temporary result cells will be open in this fashion. 
The "interpreter stack" is pointed to by R3. When a new interpreter
is sprouted, it is given a new stack area. Each interpreter has
certain status information which facilitates transfer of control
between interpreters.  This information is kept in the interpreter
status block, which is always pointed to by R4.  Most important are
the IPC, the Interpreter Program Counter, the ENV, which points to
the local environment, and LEV, which stores the current lexical
level. 

Each procedure has an environment, which is a data area holding
information vital to that procedure.  This includes pointers to all
the variables local to that procedure, and return information.
⊗

	INSTSZ == 20	;Size of an interpreter stack

;Interpreter status block
	II == 0
	XX IPC		;Interpreter program counter. Leave this as first field!
	XX ENV		;Location of local environment
	XX LEV		;Lexical level of current execution
	XX NXTINT 	;Next interpreter in the list. For GC of the stacks & env
	XX STKBAS 	;Location of start of stack area.  Needed
			;for eventual reclamation.
	XX PDB		;Location of process descriptor block (for reclamation)
	XX EVT		;The event to signal as this interpreter goes away
	XX CMCB		;Pointer to c-m control block if this is a checker or a body
	XX CMECH	;Mech bits of current device. Used by MOVE & UPDEPR
	ISBS == II/2	;Size (in words) of interpreter status block

;Procedure descriptor fields
	II == 0
	XX IPC		;Pointer to code for procedure body
	XX ENV		;Pointer to current environment when procedure is defined
	XX LEV		;Lexical level of procedure
	XX ENVSZ	;Size of environment needed by procedure
	XX NARGS	;Number of arguments for procedure
	; arg 1 - arg n	;argument list giving access & data types
	PRODSZ == II/2	;Size (in words) of procedure descriptor - minus arg list

;Procedure header fields
	II == 0
	XX NXTPRO	;Pointer to next procedure in chain (for marking purposes)
	XX OIPC		;Old IPC.  Program counter for calling process.
	XX OENV		;Old environment, the one for the calling process.
	XX OLEV		;Old level.  The lexical level of calling process.
	XX STKBAS 	;Location of start of stack area.  Needed
			;for eventual reclamation.
	XX OR3		;Old R3 - we will restore it when we return
	XX OR5		;Old R5 of calling process - R5 will point to this header
	XX OSTOP	;Old R3 stack top
	XX OSBOT	;Old R3 stack bottom
	PROHSZ == II/2	;Size (in words) of procedure header

;Fixed fields in the environment of each process
	II == 0
	XX SLINK 	;Pointer to environment of next (outer, lower
			;  numbered) block
	XX LVARS	;First location where pointers to local variables go
	ENVSIZ == II/2	;Size (in words) of environment header
	
;Each environment entry consists of two words. The first gives the accessing
;method  & the data type, while the second contains a pointer to the 
;value/header.

;   Data types
	SCLTYP == 1
	VECTYP == 2
	TRNTYP == 3
	EVNTYP == 4
	CMNTYP == 5

;   Access methods
;	DIRECT == 0		;Pointer to value
	HDRTYP == 400		;Pointer to frame header
	ARYTYP == 1000		;Pointer to array header
	REFTYP == 2000		;Indirect pointer to entry in another environment
	PROTYP == 4000		;Pointer to procedure descriptor

;   Mechanism bits.
	YARM == 1
	YHAND == 2
	BARM == 4
	BHAND == 10
	ANARM == YARM + BARM
	AHAND == YHAND + BHAND
	VISE == 20
	DRIVER == 40

;    Servo bits.
	YARMSB == 176000
	YHANDSB == 1000
	BARMSB == 770
	BHANDSB == 4
	VISESB == 2
	DRVRSB == 1

;   Table offsets for various mechanisms.
	OFYARM == 0
	OFYHAND == 6*2
	OFBARM == 7*2
	OFBHAND == 16*2
	OFVISE == 17*2
	OFDRIVER == 20*2

;  Environment offsets for the various mechanisms
	YAOFST == 0
	YHOFST == 1
	BAOFST == 2
	BHOFST == 3

;  Environment offsets for the deproach variables
;	YAPPROACH == 4
;	YDEPART   == 5
;	BAPPROACH == 6
;	BDEPART   == 7

;  Environment offsets for speed-factor
;	speed-factor == 10

;  Environment offsets for devices: vise & driver
;	VISE == 11
;	FIXEDJAW == 12
;	MOVINGJAW == 13
;	VISEOP == 14
;	DRIVERGRASP == 15
;	DRIVERTIP == 16
;	DRIVERTRANS == 17
;	TURNS == 20

;  Environment offsets for error variables
;	barm-error == 21
;	bhand-error == 22
;	yarm-error == 23
;	yhand-error == 24
;	vise-error == 25
;	driver-error == 26

DATA
SYSENV:	0	;SLINK = nil
	SYSEND	;LVARS
	HDRTYP+TRNTYP, YARMHD
	HDRTYP+SCLTYP, YHANDH
	HDRTYP+TRNTYP, BARMHD
	HDRTYP+SCLTYP, BHANDH
YAPPR:	TRNTYP, NILTRN		;yapproach
YDEPR:	TRNTYP, NILTRN		;ydepart
BAPPR:	TRNTYP, NILTRN		;bapproach
BDEPR:	TRNTYP, NILTRN		;bdepart
SPDFAC:	SCLTYP, TWO		;speed-factor
	HDRTYP+SCLTYP, VISEH	;vise opening width
	HDRTYP+TRNTYP, FJAWH	;vise fixed jaw
	HDRTYP+TRNTYP, MJAWH	;vise moving jaw
VISOP:	TRNTYP, 0		;vise opening
	HDRTYP+TRNTYP, DRVGRP	;driver grasp
	HDRTYP+TRNTYP, DRVTIP	;driver tip
	TRNTYP, 0		;driver trans (initially undefined)
	HDRTYP+SCLTYP, TURNSH	;driver turns
BAERR:	SCLTYP, 0		;barm-error
BHERR:	SCLTYP, 0		;bhand-error
YAERR:	SCLTYP, 0		;yarm-error
YHERR:	SCLTYP, 0		;yhand-error
VIERR:	SCLTYP, 0		;vise-error
DRERR:	SCLTYP, 0		;driver-error
SYSEND:	0

YARMHD:	0	;Header for YARM
	0	;type = trans device
	YARMSB	;servo bits for coefficient list
	0
	0	;calc list = nil
	YARM	;mechanism bits

YHANDH:	0	;Header for YHAND
	SCDEV	;type = scalar device
	YHANDSB	;servo bits for coefficient list
	0
	0	;calc list = nil
	YHAND	;mechanism bits

BARMHD:	0	;Header for BARM
	0	;type = trans device
	BARMSB	;servo bits for coefficient list
	0
	0	;calc list = nil
	BARM	;mechanism bits

BHANDH:	0	;Header for BHAND
	SCDEV	;type = scalar device
	BHANDSB	;servo bits for coefficient list
	0
	0	;calc list = nil
	BHAND	;mechanism bits

VISEH:	0	;Header for VISE
	SCDEV	;type = scalar device
	VISESB	;servo bits for coefficient list
	0
	0	;calc list = nil
	VISE	;mechanism bits

FJAWH:	0	;Header for vise FIXED JAW
	1	;variable
	1	;not valid
	0	;no value
	0	;calc list = nil

MJAWH:	0	;Header for vise MOVING JAW
	1	;variable
	1	;not valid
	0	;no value
	0	;calc list = nil

DRVGRP:	0	;Header for driver grasp
	1	;variable
	1	;not valid
	0	;no value
	0	;calc list = nil

DRVTIP:	0	;Header for driver tip
	1	;variable
	1	;not valid
	0	;no value
	0	;calc list = nil

TURNSH:	0	;Header for DRIVER
	SCDEV	;type = scalar device
	DRVRSB	;servo bits for coefficient list
	0
	0	;calc list = nil
	DRIVER	;mechanism bits

;INTINIT, NEWENV, MINTS, MRKENV, MRKHDR

INTEVT:	0		;The event that interlocks references to ISTBLK.
CODE

INTINIT:	;Initializes the above events
	EVMAK		;Initialize the INTEVT.
	MOV (SP),INTEVT;
	EVSIG 
	RTS PC		;Done

MINTS:	;Marking method for interpeters
	PUSH <R2,R3>		;Save R2 & R3
	EVWAIT INTEVT		;Enter critical region
	MOV NXTINT+ISTBLK,R2	;R2 ← LOC[first real interpeter status block]
	BEQ 4$			;If none, then done

1$:	JSR PC,MRKSTK		;Mark the stack

	;mark the environment
3$:	MOV ENV(R2),R3		;R3 ← environment
	JSR PC,MRKENV		;Go mark the environment
	
	MOV NXTINT(R2),R2	;R2 ← LOC[next interpreter status block]
	BNE 1$			;Repeat as necessary

	;handle currently active procedures
4$:	MOV PROLST,R2		;R2 ← LOC[first procedure header]
	BEQ 6$			;  if any
5$:	JSR PC,MRKSTK		;Mark the procedure's stack
	MOV OENV(R2),R3		;Mark the environment of the caller
	JSR PC,MRKENV
	MOV NXTPRO(R2),R2	;R2 ← LOC[next procedure header]
	BNE 5$			;Repeat as necessary

	;mark the system environment
6$:	MOV #SYSENV,R3
	JSR PC,MRKENV		;Mark the system variables
	POP <R3,R2>		;Restore R3 & R2
	EVSIG INTEVT
	RTS PC			;Return

MRKSTK:	;mark the stack pointed to by R2
	MOV STKBAS(R2),R3	;R3 ← LOC[interpreter stack base]
	ADD #2*INSTSZ,R3	;R3 ← LOC[verge of new stack] (INSTSZ is in bytes)
1$:	MOV -(R3),R0		;R0 ← stack entry
	BEQ 2$			;If 0, then end of stack (RF:  this wont work!!)
	JSR PC,MARKQ
	MOV R0,(R3)		;Put it back (compacting may move it)
	BR 1$
2$:	RTS PC

MRKENV:	PUSH <R2,LVARS(R3)>	;Save R2 & LOC[first free variable entry]
	ADD #2*ENVSIZ,R3	;R3 ← LOC[first variable entry]
1$:	CMP R3,(SP)		;See if we're done
	BHIS 20$
	TSTB 1(R3)		;Check access method
	BNE 3$
	CMP (R3),#EVNTYP	;Check if event or cmon
	BGE 2$			;  & if so don't mark it
	MOV 2(R3),R0		;Direct - R0 ← LOC[value]
	JSR PC,MARKQ		;Mark it
	MOV R0,2(R3)		;Compacting might move it
2$:	CMP (R3)+,(R3)+		;R3 ← LOC[next variable entry to mark]
	BR 1$			;Keep going

3$:	BIT #REFTYP,(R3)	;Indirect reference?
	BNE 2$			; skip it if so
	BIT #HDRTYP,(R3)	;Frame header?
	BEQ 10$			; skip ahead if not
	MOV 2(R3),R2		;R2 ← LOC[frame header]
	JSR PC,MRKHDR		;Mark it
	BR 2$
10$:	BIT #ARYTYP,(R3)	;Do we have an array to mark?
	BEQ 2$			;Skip to next if not (don't mark procedure desc)
	MOV 2(R3),R2		;R2 ← LOC[array header]
	MOV (R2)+,R1		;R1 ← # of dimensions
	MUL #6,R1		;R1 ← # bytes taken up by bounds info
	ADD R2,R1		;R1 ← LOC[beginning of array environment]
	MOV R1,R0
	MOV (R2)+,R1		;R1 ← upper bound of 1st subscript
	SUB (R2)+,R1
	INC R1			;R1 ← range of 1st subscript
	MUL (R2)+,R1		;R1 ← size of array
	MOV R0,R2		;R2 ← LOC[first entry of array environment]
	CMPB (R2),#EVNTYP	;Check datatype of array
	BGE 2$			;Don't bother marking an array of events
	PUSH <R3,R4>
	MOV R1,R4		;R4 ← # of entries to mark
	MOV R2,R3		;R3 ← LOC[first entry to mark]
11$:	BIT #HDRTYP,(R3)+	;Do we have a frame header to mark?
	BEQ 12$
	MOV (R3)+,R2		;R2← LOC[frame header]
	JSR PC,MRKHDR		;Mark the frame & its affixments
	BR 13$
12$:	MOV (R3),R0		;R0 ← LOC[value pointer]
	JSR PC,MARKQ		;Mark it
	MOV R0,(R3)+		;Compacting might move it
13$:	SOB R4,11$		;Mark everyone
	POP <R4,R3>
	BR 2$			;Done with array - go mark rest of environment

20$:	TST (SP)+		;Clean LVARS off stack
	POP <R2>		;Restore R2
	RTS PC

MRKHDR:				;R2 ← LOC[frame header to mark]
	BIT #FTYPE,TYPE(R2)	;See if device
	BEQ 1$			;Don't mark value for devices
	MOV VAL(R2),R0
	JSR PC,MARKQ		;Mark it
	MOV R0,VAL(R2)
1$:	MOV CALCS(R2),R2	;R2 ← list of affixments
	BEQ 4$			;  if any
2$:	BIT #FRAME2+EXPTRN,TYPE(R2)	;See if we should mark the trans
	BNE 3$
	MOV TRANS(R2),R0
	JSR PC,MARKQ		;Mark it
	MOV R0,TRANS(R2)
3$:	MOV (R2),R2		;Deal with the next affixment
	BNE 2$
4$:	RTS PC			;Done

NEWENV:	;Gets a new environment, returns address in R0.
	MOV @(R4),R0	;Get number of variables used in this environment
	ADD #2,(R4)	;Bump IPC
	ASL R0		;Need 2 words/variable
	ADD #ENVSIZ,R0	;Add in header size
	JSR PC,GTFREE	;Allocate from large blocks
	MOV R0,LVARS(R0)
	ADD #2*ENVSIZ,LVARS(R0)	;Initialize where the first free entry
	RTS PC				;  should go

;Interpreter itself: INTERP

	.MACRO MAKEOP CNAME, ANAME	;Compiler name, Address name
	XX	CNAME
	ANAME
	.ENDM
DATA
;The interpreter operation table
INTOPS: MAKEOP XINVALID,INVALID		;Illegal instruction
	.INSRT	INTOPS.PAL[AL,HE]
	INSEND = II	;Marks the end of the instructions
CODE
	.MACRO FETCH foo
	MOV @(R4),foo	;Get next interpreter instruction in foo
	ADD #2,(R4)	;Bump IPC
	.ENDM

	.MACRO BMPIPC
	ADD #2,(R4)	;Bump IPC
	.ENDM

	.MACRO BACKIPC
	SUB #2,(R4)	;Backup IPC
	.ENDM

	.MACRO CCC	;Clear condition code
;	CLR R0		;Clear condition code.  Not used right now.
	.ENDM

	.MACRO SCC	;Set condition code
;	MOV #2,R0	;Set condition code.  Not used right now. (maybe use TST PC)
	.ENDM

INTERP:
	MOV R3,R0	;Save limits of the interpreter stack for error checking.
	SUB #2*INSTSZ,R0 ;R0 ← Stack base (hopefully)
	PUSH <R0,R3>	;Stack bottom then top
INT1:	CMP R3,2(SP)	;Interpreter stack overflow?
	BHI 1$		;No.  Go to next instruction.
	ALERR INTMS3	;Yes.  Complain.
1$:	CMP R3,(SP)	;Interpreter stack underflow?
	BLOS 2$		;No.  Go to next instruction.
	ALERR INTMS4	;Yes.  Complain.
2$:	CLR -2(R3)	;Zero above top of stack - to keep MINTS happy
	MOV @IPC(R4),R0	;R0 ← next instruction
	BLE INVALID	;Instruction out of range
	CMP R0,#INSEND	;Is instruction too large?
	BLE INT2	;No.

.IFNZ CPOINTY
	BIT #40000,R0	; see if before an AL command
	BEQ 3$		; No.
	BMPIPC
	JSR PC,PBEG	; set up stuff to indicate beginning of AL instruction
	BR INT1
3$:	BIT #20000,R0	; see if after an AL command
	BEQ INVALID	; neither beginning or end of an AL instruction
	BMPIPC
	JSR PC,PEND
	BR INT1
.ENDC
INVALID::ALERR INTMS1	;Yes. complain.
INT2::	BMPIPC		;Bump IPC
	JSR PC,@INTOPS(R0)	;Call the appropriate routine
	BR INT1		;Repeat interpreter loop

DATA
INTMS1:: ASCIE /INTERPRETER INSTRUCTION OUT OF RANGE/
;INTMS2:: ASCIE /INTERPRETED INSTRUCTION RETURNED FAILURE/
INTMS3:: ASCIE /INTERPRETER STACK OVERFLOW/
INTMS4:: ASCIE /INTERPRETER STACK UNDERFLOW/
CODE
;  GETARG, GETENV, GETSCA, GETVEC, GETTRN

GETARG:
COMMENT ⊗ Called with R0=variable name (level-offset), returns with R0 pointing
	to the environment entry for the variable, after performing any array
	references. ⊗
	JSR PC,GETENV		;Get the environment pointer in R0
	BIT #ARYTYP,(R0)	;Do we have an array to access?
	BEQ 10$
	PUSH <R2>
	MOV 2(R0),R2		;R2 ← LOC[array header]
	MOV (R2)+,R0		;R0 ← # of dimensions of array
	CLR -(SP)		;Set offset to zero
3$:	LDF @(R3)+,AC0		;Get value of subscript
	STCFI AC0,R1		;Convert it to integer & store it in R1
	CMP R1,(R2)+		;Check we don't exceed upper bound
	BLOS 4$
	PUSH <R0>		;  no good
	ALERR 20$		;Complain
	POP <R0>
	MOV -2(R2),R1		;Use upper bound as default
4$:	SUB (R2)+,R1		;Check we also satisfy lower bound
	BHIS 5$
	PUSH <R0>		;  no good
	ALERR 21$		;Complain
	POP <R0>
	CLR R1			;Use lower bound as default
5$:	MUL (R2)+,R1		;Multiply by MULT[i]
	ADD R1,(SP)		;Update offset
	SOB R0,3$		;Do all the subscripts
	POP <R0>		;R0 ← offset into array
	ASH #2,R0		;Convert to byte offset into environment
	ADD R2,R0		;Add base address to offset
	POP <R2>
10$:	RTS PC			;Return with R0 = LOC[Env entry for variable]
DATA
20$:	.ASCIZ /Subscript index greater than upper bound./
21$:	ASCIE </Subscript index less than lower bound./>
CODE

GETENV:		;Auxiliary routine - called by GETARG & PROC (& WRIST temp)
COMMENT ⊗
 Arguments:  
   R0=variable name:  high byte is lexical level, low byte is offset.
   R4=pointer to interpreter status block.
 Result:
   R0← pointer to address of desired variable.  
   R1 clobbered.
 This routine returns in R0 a pointer to the location in the current
   environment (or, if necessary, more global environment) which
   points to the variable which is named in R0. Any indirect references
   are resolved. ⊗
	PUSH <R2>	;Save R2
	MOV R0,R1
	BIC #177400,R1	;R1 ← Offset desired
	CLRB R0	
	SWAB R0		;R0 ← Lexical level
	BNE 1$
	MOV #SYSENV,R2	;For lexical level 0 use system environment
	BR 3$
1$:	MOV ENV(R4),R2	;R2 ← LOC[local environment]
	SUB LEV(R4),R0	;R0 ← Difference in levels: desired-got
	BEQ 3$		;Diff=0; can use R2 as pointer at right base.
	BHI 6$		;If diff>0, then value inaccessible.
	NEG R0		;Make diff>0 so we can use a SOB instruction
2$:	MOV SLINK(R2),R2;Must go up a level.  R2 ← LOC[more global environment]
	SOB R0,2$	;If not yet good, then move up another level
3$:	ASH #2,R1	;Convert offset to environment pointer (each entry = 2 wds)
	ADD #2*ENVSIZ,R1	;Add in environment header
	ADD R2,R1	;R1 ← environment + offset = location of desired entry
	MOV R1,R0
	POP <R2>	;Restore R2.
4$:	BIT #REFTYP,(R0)	;An indirect reference?
	BEQ 5$
	MOV 2(R0),R0		;  yes - fetch actual reference
	BR 4$			;Check for multiple levels of indirection
5$:	RTS PC		;Done.
6$:	PUNT 7$
DATA
7$:	ASCIE /ATTEMPT TO ACCESS UNAVAILABLE VARIABLE/
CODE

GETSCA:	;Gets place for a scalar result, and places a pointer on
	;the interpreter stack.  Location is returned in R0.  
	;Simple procedure.
	MOV #SCASPC,R0
	JSR PC,GETSBK	;Allocate from small blocks
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
	EVSIG SBEVT	;End of critical section
	RTS PC		;Done

GETVEC:	;Gets place for a vector result, and places a pointer on
	;the interpreter stack.  Location is returned in R0.  
	;Simple procedure.
	MOV #VCTSPC,R0
	JSR PC,GETSBK	;Allocate from small blocks
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
	EVSIG SBEVT	;End of critical section
	RTS PC		;Done

GETTRN:	;Gets place for a trans result, and places a pointer on
	;the interpreter stack.  Location is returned in R0.  
	;Simple procedure.
	MOV #TRNSPC,R0
	JSR PC,GETSBK	;Allocate from small blocks
 	MOV R0,-(R3)	;Push new value cell pointer on interpreter stack.
	EVSIG SBEVT	;End of critical section
	RTS PC		;Done

;Variable declaration:  MVAR, KVAR;

MVAR:	;Interpreter routine

COMMENT ⊗ A list of arguments, each of which is a type/count pair. This list
is terminated by a zero entry.  For each data type entries in the environment
are created for the specified number of variables. Algebraic variables (scalar,
vector & trans) initially have no value. Events get created and their identifiers
are stored in the environment. For cmons a new condition monitor of the indicated
type is created and a pointer to its control block is placed in the environment.
Frame headers are created by AFFIX. Arrays are allocated when created. Note that
for arrays the count field actually contains the # of dimensions. For procedures
a procedure descriptor is created. ⊗

	MOV ENV(R4),R2		;R2 ← LOC[current environment]
	MOV LVARS(R2),R2	;R2 ← LOC[first free entry in environment]
1$:	FETCH R1		;R1 ← data type of variables to make
	TST R1
	BNE 2$			;  if any
	MOV ENV(R4),R0		;R0 ← LOC[environment]
	MOV R2,LVARS(R0)	;Update first free variable entry
	CCC
	RTS PC			;(who says returns should go at the end?)
2$:	FETCH R0		;Get count of # of this type of variable to make
	CMP R1,#EVNTYP		;See what data type they should be
	BEQ 4$			;  event
	BGT 5$			;  cmon, array & procedure
3$:	MOV R1,(R2)+		;For algebraic just stick data type in place
	CLR (R2)+		;  & zero the value pointer
	SOB R0,3$		;  for each one
	BR 1$
4$:	MOV R1,(R2)+		;Set data type to event
	EVMAK			;Make a new event
	POP <(R2)+>		;  & store its identifier in the environment
	SOB R0,4$		;  for each one
	BR 1$
5$:	BIT #177400,R1		;Check datatype
	BNE 6$			;  array or procedure
	JSR PC,CMMAK		;Make the new cmons & store them in environment
	BR 1$
6$:	BIT #ARYTYP,R1
	BNE 10$			;  array
	MOV R1,(R2)+		;Set access type to procedure
	PUSH <R0>
	FETCH R0		;Get # of arguments for procedure
	PUSH <R0>		;Save it for later
	ADD #PRODSZ,R0		;R0 ← size of procedure descriptor (in words)
	JSR PC,GTFREE		;R0 ← LOC[procedure descriptor]
	MOV R0,(R2)+		;Store pointer to procedure descriptor in env
	FETCH (R0)+		;Get IPC of procedure body
	MOV ENV(R4),(R0)+	;Set up pointer to environment outside procedure
	MOV LEV(R4),(R0)	;Set up lexical level of procedure
	INC (R0)+		;  to one greater than creating interpreter
	FETCH (R0)		;Get size of environment needed
	ASL (R0)		;Need 2 words/variable
	ADD #ENVSIZ,(R0)+	;Add in header size
	POP <R1>		;Retrieve # of args
	MOV R1,(R0)+		; & save it in procedure descriptor
	BEQ 8$			;  if any
7$:	FETCH (R0)+		;Get the access/datatype info for each arg
	SOB R1,7$
8$:	POP <R0>		;Restore count of procedures to make
	SOB R0,6$		;  and make them all
	BR 1$
10$:	MOV R1,(R2)+		;Indicate that this is an array
	PUSH <R2,R1,R0>		;Save # of dimensions & type of array to make
	ASL R0			;Will need two words per subscript
	JSR PC,GTFREE		;Get some temporary space to store bounds info
	MOV R0,R2		;R2 ← LOC[temp storage]
	MOV (SP),R0		;R0 ← # of dimensions in array
	PUSH <#2>		;(SP) ← size needed for array (2 words/entry)
11$:	PUSH <R0>
	JSR PC,GTBND		;Get upper bound
	JSR PC,GTBND		;Get lower bound
	POP <R0>
	MOV -4(R2),R1		;Compute upper-lower+1
	SUB -2(R2),R1
	INC R1
	MUL (SP),R1		;(upper-lower+1) * size
	MOV R1,(SP)		;Update array size
	SOB R0,11$		;Handle all of the subscript bounds
	POP <R0>		;R0 ← array environment size
	MOV (SP),R1		;R1 ← # of dimensions in array
	MUL #3,R1		;Need three words per dimension
	INC R1			;An extra word for the # of dimensions
	ADD R1,R0		;R0 ← Size for array header + environment
	ASL R1			;R1 ← Size of header in bytes
	PUSH <R1>
	JSR PC,GTFREE		;R0 ← LOC[array header]
	MOV R0,@6(SP)		;Store pointer to array header into environment
	ADD (SP)+,R0		;R0 ← LOC[beginning of environment]
	MOV (SP),R1		;R1 ← # of dimensions
	PUSH <R0>
	MOV #1,-(R0)		;Multiplier for nth subscript is one
12$:	PUSH <R1>
	MOV -(R2),-(R0)		;Put lower bound in place
	MOV -(R2),R1		;Get upper bound
	MOV R1,-(R0)		;Put it away
	SUB 2(R0),R1
	INC R1			;  upper - lower + 1
	MUL 4(R0),R1		;Compute next multiplier
	MOV R1,-(R0)		;  & store it
	POP <R1>
	SOB R1,12$		;Repeat for each dimension of array
	PUSH <R0>
	MOV R2,R0		;R0 ← LOC[temp storage]
	JSR PC,RLFREE		;Deallocate temp storage
	POP <R0,R1>		;R0 ← LOC[array header], R1 ← LOC[array env]
	MOV (R0),R2		;R2 ← Size of the array
	POP <(R0)>		;Store # of dimensions into array header
	POP <R0>		;R0 ← datatype
	BIC #ARYTYP,R0
	CMP #EVNTYP,R0		;An event array?
	BEQ 14$
13$:	MOV R0,(R1)+		;Store datatype
	CLR (R1)+		;Zero value pointer
	SOB R2,13$		;Do the whole array
	BR 15$
14$:	MOV R0,(R1)+		;Store datatype
	EVMAK			;Make the event
	POP <(R1)+>		; & store it away
	SOB R2,14$		;Do the whole array
15$:	POP <R2>		;Restore R2
	TST (R2)+		;R2 ← LOC[next environment entry]
	JMP 1$			;DONE!!! Go see about making more variables

GTBND::	FETCH R0		;Get bound
	TST R0			;See if constant or variable (level-offset)
	BMI 1$
	JSR PC,GETARG		;R0 ← LOC[variable]
	LDF @2(R0),AC0		;AC0 ← variable value (better be a scalar)
	STCFI AC0,(R2)+		;Store the upper bound away
	BR 2$
1$:	ASL R0			;Get rid of the constant bit
	ASR R0			;But be sure to preserve the correct sign
	MOV R0,(R2)+
2$:	RTS PC

KVAR:	;Interpreter routine

COMMENT ⊗ Given the number of variables to kill. The last ones in the
current environment are destroyed. For each frame an attempt is made to
validate any dependents first. ⊗

	FETCH R1		;Get # of variables to kill
KVAR0::				;Entry point for procedure returns
	MOV ENV(R4),R2		;R2 ← LOC[environment]
	MOV LVARS(R2),R2	;R2 ← LOC[first free variable entry]
1$:	SUB #4,R2		;R2 ← LOC[variable to kill]
	TSTB 1(R2)		;Test access method
	BEQ 10$			;Do the direct accesses below
	BIT #REFTYP,(R2)+	;An indirect reference?
	BNE 12$			;  yes - don't do anything
	BIT #HDRTYP,-(R2)	;A frame?
	BEQ 2$
	TST (R2)+
	JSR PC,KFRAME		;Kill the frame & its affixments
	BR 12$
2$:	BIT #PROTYP,(R2)	;A procedure descriptor?
	BEQ 3$
	TST (R2)+
	PUSH <R1>
	MOV (R2),R0
	JSR PC,RLFREE		;Deallocate the procedure descriptor
	POP <R1>
	BR 12$
3$:	BIT #ARYTYP,(R2)+	;An array?
	BEQ 12$			;  no - not an access method that needs killing
	PUSH <R1,R2>
	MOV (R2),R2		;R2 ← LOC[array header]
	MOV (R2)+,R1		;R1 ← # of dimensions
	MUL #6,R1		;R1 ← # bytes taken up by bounds info
	ADD R2,R1
	MOV R1,R0		;R0 ← LOC[beginning of array environment]
	MOV (R2)+,R1		;R1 ← upper bound of 1st subscript
	SUB (R2)+,R1
	INC R1			;R1 ← range of 1st subscript
	MUL (R2)+,R1		;R1 ← size of array = # of entries to kill
	MOV R0,R2		;R2 ← LOC[first entry of array environment]
	CMPB #TRNTYP,(R2)	;Check datatype of array
	BGT 9$			;Don't bother killing scalars & vectors
	BEQ 5$
4$:	TST (R2)+		;Skip over type word
	EVKIL (R2)+		;Kill the event
	SOB R1,4$		;Get all of them
	BR 9$
5$:	BIT #HDRTYP,(R2)+	;Do we have a frame header to kill?
	BEQ 6$
	JSR PC,KFRAME		;Kill the frame & its affixments
6$:	TST (R2)+		;Point to next environment entry
	SOB R1,5$		;Kill everyone
9$:	POP <R2>		;Restore old environment pointer
	MOV (R2),R0
	JSR PC,RLFREE		;Deallocate array header
	POP <R1>		;Restore old kill count
	BR 12$
10$:	CMP (R2)+,#EVNTYP	;What type is it?
	BLT 12$			;Algebraic types are easy
	BGT 11$
	EVKIL (R2)		;Kill the event
	BR 12$
11$:	JSR PC,CMDEST		;Kill the cmon
12$:	CLR (R2)		;Zero the pointer field
	CLR -(R2)		;Zero the type field
	DEC R1
	BGT 1$			;Kill all that we were asked to
	MOV ENV(R4),R0		;R0 ← LOC[environment]
	MOV R2,LVARS(R0)	;Update first free variable entry
	CCC		;Clear condition code
	RTS PC		;Done

;Stack ops: GTVAL, CHNGE, PUSHV, POPV

GTVAL:
COMMENT ⊗ The argument is a level-offset pair.  The variable
referenced by that pair is examined and a pointer to its value cell
is placed on the stack. ⊗
	FETCH R0	;Pick up level-offset name of argument
GTVAL0:			;Entry for MOVARG & PARK
			;also for GVALS in PINTRP.PAL
	JSR PC,GETARG	;R0 ← LOC[variable environment entry]
GVAL1:			;MSM entry point for POINTY routine GTARR in PINTRP.PAL
	BIT #HDRTYP,(R0);Check access method
	BNE 1$
	MOV 2(R0),-(R3)	;Direct - push value pointer on stack
	BNE 4$		;If it had a value all done
	CMP #TRNTYP,(R0)
	BEQ 2$		;Use niltrans for default
	MOV #NILVEC,(R3);Use vector/scalar zero
	BR 3$
1$:	MOV 2(R0),R0	;R0 ← LOC[frame header]
	JSR PC,NOCMP	;Don't compact for a bit
	CALL GETVAL,<R0>;R0 ← value
	MOV R0,-(R3)	;Push value on interpreter stack.
	JSR PC,YESCMP	;OK to compact now
	TST (R3)	;Check that we got a valid value
	BNE 4$
2$:	MOV #NILTRN,(R3);If not use the niltrans
3$:	ALERR GTVMES	;  & complain
4$:	CCC		;Clear condition code.
	RTS PC		;Done
DATA
GTVMES::ASCIE </NO VALUE FOR VARIABLE - USING DEFAULT./>
CHNERR::ASCIE </CANNOT ASSIGN VALUES TO DEVICES OR FRAMES AFFIXED TO DEVICES./>

NILTRN:
NILROT:	.FLT2 1.0, 0.0, 0.0	;Define the default values
ZERO:	.FLT2 0.0, 1.0, 0.0	;XHAT,YHAT,ZHAT labels added by MSM 12/11/78
	.FLT2 0.0, 0.0, 1.0
NILVEC:	.FLT2 0.0, 0.0, 0.0
CODE

CHNGE:
COMMENT ⊗ Pops the value from top of stack into the variable specified
by the level-offset pair given in the argument.  ⊗
	FETCH R0	;Pick up level-offset name of argument
CHNG0:			;MSM label for POINTY
	JSR PC,GETARG	;R0 ← LOC[variable environment entry]
CHNG1:			;MSM label for POINTY
	BIT #HDRTYP,(R0);Chech access method
	BNE 1$
	MOV (R3)+,2(R0)	;Direct - store value pointer away
	BR 5$
1$:	MOV 2(R0),R0	;R0 ← LOC[Desired frame header]
	BIT #FTYPE,TYPE(R0) ;Can't change values of devices
	BEQ 2$		;It's a device - go complain
	BIT #DYNAM,TYPE(R0) ;Better not be a dynamic frame either
	BEQ 3$		;It's a regular frame - go change it
2$:	ALERR CHNERR	;Can't change value of device or dynamic frame
	BR 4$
3$:	JSR PC,NOCMP	;Don't compact for a bit
	CALL CHANGE,<R0,(R3)>
	JSR PC,YESCMP	;OK to compact now
4$:	TST (R3)+	;Pop stack
5$:	CCC		;Clear condition code.
	RTS PC		;Done


PUSHV:	FETCH -(R3)	;Put argument directly on stack
	CCC		;Clear condition code.
	RTS PC		;Done

POPV:	TST (R3)+	;Pop argument off of stack
	CCC		;Clear condition code.
	RTS PC		;Done

;Flow-of-control: PROC, RETURN

PROC:
COMMENT ⊗ The arguments are the level-offset of the procedure being called, and
the list of parameters for the procedure. The procedure's arguments can come in
three flavors: on the R3 stack for expressions, an address pointing to a constant
or a level-offset for a variable. Variables can be passed either by value or
reference. Arrays are always passed by reference. This routine creates a new
procedure header, switches contexts (environments), allocates an environment
for the procedure & binds the arguments. When done R5 will point to the new
procedure header, so RETURN will know who to return from. ⊗
	FETCH R0		;Get level-offset of procedure descriptor
	JSR PC,GETENV
	MOV 2(R0),R2		;R2 ← LOC[procedure descriptor]
	MOV #PROHSZ,R0
	JSR PC,GTFREE		;R0 ← LOC[new procedure header]
	MOV R5,OR5(R0)		;Save old R5 value
	MOV R0,R5		;R5 ← LOC[currently active procedure]
	MOV (R4),OIPC(R5)	;Save IPC of caller (points to arg list)
	MOV (R2)+,(R4)		;Set up IPC of procedure body
	MOV (R2)+,OENV(R5)	;temporarily store environment of proc's parent
	MOV (R2)+,OLEV(R5)	;Lexical level of procedure
	MOV (R2)+,R0		;R0 ← Size of environment needed by procedure
	JSR PC,GTFREE		;R0 ← LOC[environment for procedure]
	MOV OENV(R5),(R0)	;SLINK ← environment of procedure's parent block
	MOV R0,OENV(R5)
	CMP (R0)+,(R0)+		;R0 ← LOC[first entry in environment]
	PUSH <(R2),R0>		;Save # of args & LOC[first entry]
	MOV (R2)+,R1		;R1 ← # of arguments for procedure
	BEQ 2$			;  if any
1$:	MOV (R2)+,(R0)+		;Copy type info from procedure descriptor → env
	TST (R0)+		;Skip over value pointer
	SOB R1,1$		;Get all of them
2$:	POP <R2>		;R2 ← LOC[first environment entry]
	MOV R0,-2(R2)		;LVARS ← LOC[first free entry]
	JSR PC,NOCMP		;Don't compact for a while
	POP <R1>		;R1 ← # of arguments for procedure
	BEQ 11$			;  if any
3$:	PUSH <R1>
	MOV @OIPC(R5),R0	;R0 ← next arg
	ADD #2,OIPC(R5)		;Bump IPC
	CMP #-1,R0		;See if stack reference
	BNE 4$
	BIC #REFTYP,(R2)+	;Make it call by value for expressions
	MOV (R3)+,(R2)+		;Pop value off of stack into environment
	BR 10$
4$:	BIT #140000,R0		;Check if label - NOTE: this hack won't work if
	BEQ 5$			;	the PCODE is ever put below 40 000
	BIC #REFTYP,(R2)+	;Make it call by value for constants
	MOV R0,(R2)+		;Store constants address in environment
	BR 10$
5$:	BIT #ARYTYP,(R2)	;See if it's an array reference
	BEQ 6$
	JSR PC,GETENV		;R0 ← LOC[environment entry for array variable]
	BIS #REFTYP,(R2)+	;Arrays are always passed by reference
	MOV R0,(R2)+		;Store pointer to array variable
	BR 10$
6$:	JSR PC,GETARG		;R0 ← LOC[environment entry for variable]
	BIT #REFTYP,(R2)+	;Call by value or reference?
	BEQ 7$
	MOV R0,(R2)+		;Call by reference - store LOC[var env entry]
	BR 10$
7$:	BIT #HDRTYP,(R0)+	;Call by value - see if direct access or header
	BNE 8$
	MOV (R0),(R2)+		;Direct - store value pointer
	BR 10$
8$:	CALL GETVAL,<(R0)>	;Header - get frame's value
	MOV R0,(R2)+		;Store value pointer
10$:	POP <R1>
	SOB R1,3$		;Bind each argument of procedure
11$:	EVWAIT INTEVT
	MOV PROLST,NXTPRO(R5)	;Link us onto active procedure list
	MOV R5,PROLST
	MOV R3,OR3(R5)		;Save stack so we can restore it on return
	MOV 4(SP),OSTOP(R5)	;Save old stack limits: overflow
	MOV 2(SP),OSBOT(R5)	; + underflow
	MOV #INSTSZ,R0		;R0 ← Size needed for an interpreter stack
	JSR PC,GTFREE		;R0 ← LOC[new interpreter stack]
	MOV R0,STKBAS(R5)	;Store away new stack base
	MOV R0,4(SP)		;So INT can check for stack overflow
	ADD #2*INSTSZ,R0	;R0 ← LOC[top of new stack] (INSTSZ is in bytes)
	MOV R0,2(SP)		;So INT can check for stack underflow
	MOV R0,R3		;R3 ← top of new stack
	MOV OENV(R5),R1
	MOV ENV(R4),OENV(R5)	;Swap out caller's environment
	MOV R1,ENV(R4)		;  & swap in procedure's
	MOV OLEV(R5),R1
	MOV LEV(R4),OLEV(R5)	;Ditto with lexical levels
	MOV R1,LEV(R4)
	EVSIG INTEVT
	JSR PC,YESCMP		;Okay to compact now
	CCC
	RTS PC			;Done - Go interpret the procedure.

RETURN:
COMMENT ⊗ When returning from a procedure kill all of the local variables in
the procedure's environment, release the environment, & swap back in the
environment of the caller. Also remove procedure from PROLST & deallocate
the procedure header. ⊗

	TST R5			;See if R5 points to a procedure header
	BEQ 10$			; Doesn't point anywhere!
	CMP -2(R5),#-<2*PROHSZ+4> ;Is the block the right size?
	BEQ 1$
10$:	ALERR RETMES		;No!!! - we're in big trouble now!!!
	BMPIPC			;Skip over value to return flag
	RTS PC			;Just ignore the return if user proceeds - good luck

1$:	MOV ENV(R4),R0		;R0 ← LOC[procedure's environment]
	TST (R0)+		;R0 ← LOC[LVARS]
	MOV (R0)+,R1		;R1 ← LOC[first free slot in environment]
	SUB R0,R1		;Length of used entries in bytes
	ASH #-2,R1		;Divide by four to get # of environment entries
	BEQ 2$			;If any
	JSR PC,KVAR0		;Go kill all the procedure's local variables
2$:	MOV ENV(R4),R0
	JSR PC,RLFREE		;Release the procedure's environment
	FETCH R0		;See if we return a value
	TST R0
	BEQ 3$
	MOV (R3)+,R0		;Get the pointer to the value
3$:	MOV OR3(R5),R3		;restore old R3 stack
	TST R0
	BEQ 4$
	MOV R0,-(R3)		;Push return value on stack
4$:	MOV STKBAS(R5),R0
	JSR PC,RLFREE		;Release the procedure's stack
	MOV OENV(R5),ENV(R4)	;Restore environment of caller
	MOV OLEV(R5),LEV(R4)	; and lexical level
	MOV OIPC(R5),(R4)	; IPC too
	MOV OSTOP(R5),4(SP)	;Restore old stack limits: overflow
	MOV OSBOT(R5),2(SP)	; + underflow
	EVWAIT INTEVT
	MOV #PROLST,R0		;Remove us from active procedure list
5$:	CMP (R0),R5		;Found us yet?
	BEQ 6$
	MOV (R0),R0		;Try next
	BNE 5$
	BR 7$			;We weren't there!
6$:	MOV (R5),(R0)		;Unlink us
7$:	EVSIG INTEVT
	MOV R5,R0
	MOV OR5(R5),R5		;Restore old R5 value
	JSR PC,RLFREE		;Release procedure header
	CCC
	RTS PC			;Done - return to caller

DATA
RETMES:: ASCIE </No where to RETURN to!!! - continuing will ignore RETURN/>
CODE
;  ABORT, GODDT, NOOP, FORCHK, FOREND, CASE, JUMP, JUMPC

ABORT:
;Aborts current motions
;This should be cleaned up sometime.
	MOV	#20,R1	;First stop everyone: 2 arms (6 jts ea), 2 hands & 2 devices
	MOV	LDVCPTR,R0	;R0 ← LOC[table of device pointers]
1$:	MOV	(R0)+,R2	;R2 ← device block
	BEQ	2$		;If any
	BIS	#100000,@0(R2)	;Stop this device.
2$:	SOB	R1,1$		;Repeat	till all devices stopped
	;SLEEP	#144		;Should pause for a bit (1/10 sec) here but...
				;  if anything gets printed no problem
	CCC			;Clear the condition codes
	RTS	PC		; & Return

GODDT:	BPT			;break to DDT
NOOP:	CCC			;Clear the condition codes
	RTS	PC		; and Return

FORCHK:		; change parallel routine in PINTRP.PAL when you change this
;Assume that the stack has, from surface in, the increment, the
;  final value, and the control variable's value, all of which are
;  scalar values.  If (FINAL-CONVAR)*(INCREMENT) ≥ 0 then this is a
;  no-op; otherwise, jump to the destination (end of FOR body) & clean up stack
;Arguments:  destination.
	FETCH R0	;Pick up level-offset name of control variable
	JSR PC,GETARG	;R0 ← LOC[variable environment entry]
	MOV 4(R3),2(R0)	;Store pointer to current value
	LDF @2(R3),AC0	;AC0 ← final value
	SUBF @4(R3),AC0	;AC0 ← final - current
	MULF @(R3),AC0	;AC0 ← (final - current)*increment
	FETCH R0	;R0 ← destination
	CFCC
	BGE 1$		;Shall this be a no-op?
	MOV R0,IPC(R4)	;No; set new IPC.
	ADD #6,R3	;Pop the inc, final & control var off of the stack
1$:	CLR R0
	RTS PC		;Done

FOREND:	;Interpreter routine
;Assume that the stack has, from surface in, the increment, the
;  final value, and the control variable's value, all of which are
;  scalar values. Copy the step size and the current value, add them
;  and replace the current value. Then jump to the start of the loop.
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3),-(R3)	;Copy step size
	MOV 6(R3),-(R3)	;Copy current value
	JSR PC,SADD	;Add them
	MOV (R3)+,4(R3)	;Update the current value
	JSR PC,YESCMP	;Okay to compact again
	BR JUMP		;Now jump to start of for loop

CASE:	;Interpreter routine
;CASE statement. Case index on stack. Takes two arguments: the permisible
;range of the index & a list of addresses telling where each of the
;statements starts.
	LDF @(R3)+,AC0	;Fetch case index
	FETCH R2	;R2 ← error index
	STCFI AC0,R0	;R0 ← case index converted to integer
	BPL 1$
	MOV R2,R0
	NEG R0		;Check if there's an ELSE statement (error index < 0)
	BPL 3$		;  yup - go do ELSE
	ALERR 4$	;Case index is negative - complain
	MOV R2,R0
	BR 3$		;Go skip to end of the case statement
1$:	MOV R2,R1	;R1 ← Maximum legal index
	BPL 2$
	NEG R1		;Make sure we use |range|, it's negative if ELSE stmnt
2$:	CMP R0,R1	;Check that index is within legal range
	BLT 3$
	MOV R2,R0
	NEG R0		;Check if there's an ELSE statement (error index < 0)
	BPL 3$		;  yup - go do ELSE
	ALERR 5$	;Case index is too large - complain
	MOV R2,R0	;Go skip to end of the case statement
3$:	ASL R0		;Convert R0 to byte offset
	ADD R0,(R4)	;IPC points to address of statement to interpret
	MOV @(R4),(R4)	;Jump to proper statement
	CCC		;Clear condition code.
	RTS PC		;Done
DATA
4$:	.ASCIZ /CASE INDEX NEGATIVE/
5$:	 ASCIE /CASE INDEX TOO LARGE/
CODE

JUMP:
;Takes one argument: the new address.
	MOV @(R4),(R4)	;Jump to new address
	CCC		;Clear condition code.
	RTS PC		;Done

JUMPC:	;Interpreter routine
COMMENT ⊗ Takes one argument: the destination address. 
The condition queries the top of the stack and pops it, assuming it
to be a scalar.  The interpreter jumps to the destination address if
the value of the scalar is false (0). rewritten 9-14-76 by arg ⊗
	LDF	@(R3)+,AC0	;Get value of boolean
	CFCC			;copy condition codes
	BEQ	1$		;if false succeed - take branch
	BMPIPC			;skip over address
	RTS	PC		; & return
1$:	MOV	@IPC(R4),IPC(R4); branch
	RTS	PC		; & return

;  SPAWN, SPROUT, TERMINATE


SPAWN:	;Utility routine

COMMENT ⊗ Takes two arguments: In R0, the IPC of the interpreter to
spawn, and in R1, the event (if any) to put in EVT of the new
interpreter.  The inferior will have the same environment as the
superior.  Creates an interpreter status block, stack, process
descriptor, and is ready for a SCHEDU when it returns the process
descriptor in R0. ⊗

	PUSH <R1,R0>		;Save the EVT & the new IPC
	MOV #ISBS,R0		;R0 ← Size (in words) of an interpreter status block
	JSR PC,GTFREE		;R0 ← LOC[new interpreter status block]
	POP <IPC(R0)>		;new IPC ← first argument
	MOV ENV(R4),ENV(R0)	;new ENV ← old ENV
	MOV LEV(R4),LEV(R0)	;new LEV ← old LEV
	EVWAIT INTEVT		;Interlock sensitive operation.
	MOV #NXTINT+ISTBLK,R1	;Link into the interpreter list.
	MOV (R1),NXTINT(R0)
	MOV R0,(R1)
	EVSIG INTEVT		;End of interlock
	POP <EVT(R0)>		;new EVT ← second argument.
	PUSH <R0>		;Save LOC[new interpreter status block]
	MOV #INSTSZ,R0		;R0 ← Size needed for an interpreter stack
	JSR PC,GTFREE		;R0 ← LOC[new interpreter stack]
	POP <R1>		;R1 ← LOC[new interpreter status block]
	MOV R0,STKBAS(R1)	;Store away new stack base
	ADD #2*INSTSZ,R0	;R0 ← LOC[top of new stack] (INSTSZ is in bytes)
	PUSH <R1,R0>		;Save R1 & R0
	MOV #210,R0		;Room for process descriptor
	JSR PC,GTFREE		;R0 ← LOC[new process descriptor]
	MOV #UFPUSE+UGRSAV+2,PDBSTA(R0)	;Use floating point,saved registers, pri=1
	POP <PDBR3(R0),R1>	;Store away new interp stack pointer (reg 3)
				;R1 ← LOC[new ISB]
	MOV R0,PDB(R1)		;Store away LOC[PDB] in new ISB
	MOV R1,PDBR4(R0)	;Store away LOC[ISB] in reg 4 of PDB
	MOV R0,USKMIN(R0)	;Set up min pointer for SP
	ADD #UFEC+36,USKMIN(R0)
	MOV R0,USKMAX(R0)	;Set up max pointer for SP
	ADD #420,USKMAX(R0)
	MOV #144040,UPSW(R0)	;Set up psw
	MOV PDB(R4),R1		;Use same UIMAP that we are using.
	MOV UIMAP(R1),UIMAP(R0)

	RTS PC		;Done

; These are the appropriate scheduling commands:
;    SCHEDU R0,#INTERP,#USRDM,#2   ;Cause the new process to be started, suspended
;    FORK R0,#INTERP,#USRDM	   ;Cause the new process to be started.

SPROUT:	;Interpreter routine

COMMENT ⊗ Arguments: One address in pseudo-code for each of the
several forks starting up, followed by a 0 word.  This is to be used
only for cobegins, not for servos.  Each new interpreter is given an
interpreter status block and is then scheduled.  As each terminates,
it signals its defining event.  Since each of these has the same
event, the current interpreter need only wait until they all happen.
⊗

	PUSH <R3>	;Save R3.  Caution:  cannot use interpreter stack now.
	CLR R3		;R3 is the count of how many inferiors to spawn.
	EVMAK		;-(SP) ← Event identifier for communication with infs.
1$:	FETCH R0	;R0 ← next argument (IPC)
	TST R0
	BEQ 2$		;If zero, then we have spawned all the inferiors.
	INC R3		;Count it.
	MOV (SP),R1	;R1 ← event for the inferior EVT
	JSR PC,SPAWN
	MOV R0,R2	;R2 ← new process control block 
	;Set up the new environment
	JSR PC,NEWENV	;R0 ← LOC[new environment]
	MOV ENV(R4),SLINK(R0)	;Not necessary to set up OLEV, etc.
	MOV PDBR4(R2),R1
	MOV R0,ENV(R1)
	INC LEV(R1)
	SCHEDU R2,#INTERP,#USRDM,#2 ;Cause the new process to be started, suspended
	BR  1$		;Go handle the next inferior.
2$:	DEC R3		;Another wait to be done?
	BMI 3$		;No, we are finished.
	EVWAIT (SP)	;Wait for an inferior to come back.
	BCC 2$		;If all well, wait for the next one.
	ALERR SPRMES	;The event was killed!
3$:	EVKIL (SP)+	;Kill the event now, remove from stack
	POP <R3>	;Restore R3
	CCC		;Clear condition code.
	RTS PC		;Done
DATA
SPRMES::ASCIE /BAD RETURN FROM INFERIOR/
CODE


TERMINATE:	
COMMENT ⊗ Interpreter routine, sometimes jumped to from other
interpreter routines.  End this interpreter.  ⊗
	MOV EVT(R4),R0		;R0 ← event to announce imminent demise
	BEQ 1$			;If there is one
	EVSIG R0		;Announce that we are about to disappear.
1$:	MOV STKBAS(R4),R0	;Reclaim interpreter stack
	JSR PC,RLFREE
	MOV ENV(R4),R0		;Reclaim this environment
	JSR PC,RLFREE
	PUSH <PDB(R4)>		;Save LOC[this PDB]
	MOV R4,R0		;Reclaim Interpreter Status Block
	JSR PC,RLFREE
	EVWAIT INTEVT		;Enter critical region.
	MOV #ISTBLK,R0	;The following unlinks this interpreter from the chain.
2$:	MOV R0,R1
	MOV NXTINT(R1),R0
	CMP R0,R4		;Have we found ours yet?
	BNE 2$
	MOV NXTINT(R4),NXTINT(R1)	; Yes. rechain.
	EVSIG INTEVT		;Leave critical region.
	POP <R0>		;Reclaim process control block (should be safe now)
	CMP R0,FREEST		;Make sure that it points into free storage.
	BLE 3$			; (it may be statically allocated)
	CMP R0,#FREEND
	BGE 3$
	JSR PC,RLFREE
3$:	DISMIS			;Go away

;Booleans: SLE,SLT,SGE,SGT,SEQ,SNE,AND,LOR,NOT,LXOR,EQV

COMP:	;auxiliary function used by SLE,SLT,SGE,SGT,SEQ,SNE
	LDF	@(R3)+,AC0	;Get first arg
	CMPF	@(R3)+,AC0	;Compare it with second arg (1st-2nd)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	MOV	ONE,(R0)+	;assume true (1.0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags from compare
	RTS	PC		; & Return

SLT:	JSR	PC,COMP		;compare the args
	BLT	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SLE:	JSR	PC,COMP		;compare the args
	BLE	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SGT:	JSR	PC,COMP		;compare the args
	BGT	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SGE:	JSR	PC,COMP		;compare the args
	BGE	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SEQ:	JSR	PC,COMP		;compare the args
	BEQ	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

SNE:	JSR	PC,COMP		;compare the args
	BNE	1$		;if true then done
	CLR	@(R3)		;else set answer to false (0)
1$:	RTS	PC		; & return

AND:	LDF	@(R3)+,AC0	;Get first arg
	LDF	@(R3)+,AC1	;Get second arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	CLR	(R0)+		;assume false (0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags for 2nd arg
	BEQ	1$		;if it's false return false
	TSTF	AC0		;else look at 1st arg
	CFCC
	BEQ	1$
	MOV	ONE,@(R3)	;if both args are true return true (1.0)
1$:	RTS	PC		; Return

LOR:	LDF	@(R3)+,AC0	;Get first arg
	LDF	@(R3)+,AC1	;Get second arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	MOV	ONE,(R0)+	;assume true (1.0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags from compare
	BNE	1$		;if it's true return true
	TSTF	AC0		;else look at 1st arg
	CFCC
	BNE	1$
	CLR	@(R3)		;if both args are false return false (0)
1$:	RTS	PC		; Return

NOT:	LDF	@(R3)+,AC0	;Get arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	CLR	(R0)+		;assume false (0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags for arg
	BNE	1$		;if it's false return true
	MOV	ONE,@(R3)	; else return true
1$:	RTS	PC		; Return

EQV:	LDF	@(R3)+,AC0	;Get first arg
	LDF	@(R3)+,AC1	;Get second arg (and set condition flags)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC, GETSCA	;R0 ← -(R3) ← LOC[new_scalar]
	MOV	ONE,(R0)+	;assume true (1.0)
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CFCC			;copy condition flags from compare
	BEQ	1$		;if it's true check that other is
	TSTF	AC0		;2nd arg is false look if 1st arg is too
	CFCC
	BNE	2$		;Nope not both false
	BR	3$		;Yup both false
1$:	TSTF	AC0		;2nd arg is true look if 1st arg is too
	CFCC
	BNE	3$		;Both are true
2$:	CLR	@(R3)		;if both args aren't the same return false (0)
3$:	RTS	PC		; Return

LXOR:	JSR PC,EQV		;Compute equivalence relation of the 2 args
	JMP NOT			;  & negate it (⊗ = ¬≡), then return

;return scalars: SABS,SADD,SSUB,SMUL,SDIV,SNEG,SEXP,MAX,MIN,INT,IDIV,MOD

COMMENT ⊗ All timings are averages of 1000 runs.  They take into
account the cost of the RTS but not the JSR.  It is assumed that
GETSCA and GETVEC take no time.  All routines on this page are
interpreter routines.  ⊗

SABS:	;Scalar ← |Scalar|
	LDF @(R3)+,AC0	;AC0 ← arg 
	ABSF AC0	;AC0 ← |arg|
	BR SRET		;Store result & return

;30 microseconds
SADD:	;Scalar ← Scalar + Scalar
	LDF @(R3)+,AC0	;AC0 ← arg 2
	ADDF @(R3)+,AC0	;AC0 ← arg2 + arg1
	BR SRET		;Store result & return

SSUB:	;Scalar ← Scalar - Scalar
	LDF @2(R3),AC0	;AC0 ← arg 1
	SUBF @(R3)+,AC0	;AC0 ← arg1 - arg2
	TST (R3)+	;Move past first argument
	BR SRET		;Store result & return

;30 microseconds
SMUL:	;Scalar ← scalar * scalar
	LDF @(R3)+,AC0	;AC0 ← arg 2
	MULF @(R3)+,AC0	;AC0 ← arg2 * arg1
	BR SRET		;Store result & return

;33 microseconds
SDIV:	;Scalar ← Scalar / Scalar
	LDF @(R3)+,AC1	;AC1 ← arg 2
	LDF @(R3)+,AC0	;AC0 ← arg 1
	DIVF AC1,AC0	;AC0 ← arg1 / arg2
	BR SRET		;Store result & return

;26 microseconds
SNEG:	;Scalar ← -Scalar
	LDF @(R3)+,AC0	;AC0 ← arg
	NEGF AC0	;AC0 ← -arg
	BR SRET		;Store result & return

INT:	LDF @(R3)+,AC0	;AC0 ← arg
	STCFI AC0,R0	;R0 ← Integer part of arg
	LDCIF R0,AC0	;Float the integer part
	BR SRET

DIVMOD:	LDF @(R3)+,AC0	;AC0 ← 2nd arg
	STCFI AC0,-(SP)	;push integer part of it
	LDF @(R3)+,AC0	;AC0 ← 1st arg
	STCFI AC0,R0	;R0 ← integer part of it
	ASHC #-20,R0	;R0,R1 ← 32 bit integer part of 1st arg
	DIV (SP)+,R0	;Divide 2nd arg into 1st. R0 ← quotient, R1 ← remainder
	RTS PC

IDIV:	JSR PC,DIVMOD	;Do common code
	LDCIF R0,AC0	;  & float quotient
	BR SRET

MOD:	JSR PC,DIVMOD	;Do common code
	LDCIF R1,AC0	;  & float remainder
SRET::	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar block]
	STF AC0,@(R3)	;Store result
	CCC		;Clear condition code.
	RTS PC		;Done

MAX:	LDF @(R3)+,AC0	;Get 2nd arg
	CMPF @(R3),AC0	;Compare 1st against 2nd
	CFCC		;Copy condition codes. N set if 1st<2nd
	BPL 1$
	MOV -2(R3),(R3)	;2nd was max
1$:	CCC
	RTS PC		;Done

MIN:	LDF @(R3)+,AC0	;Get 2nd arg
	CMPF @(R3),AC0	;Compare 1st against 2nd
	CFCC		;Copy condition codes. N set if 1st<2nd
	BMI 1$
	MOV -2(R3),(R3)	;2nd was min
1$:	CCC
	RTS PC		;Done

SEXP:	;Scalar ← Scalar ↑ Scalar
	TSTF @2(R3)	;Check sign of base
	CFCC
	BGT 3$		;Positive base - no sweat
	BMI 2$		;Negative base - see if exponent is integer or not
	TST @(R3)+	;Zero base - check sign of exponent 
	CLRF AC0	; Result is zero if exponent is positive
	BPL 1$
	LDF INF,AC0	;  or  +infinity if exponent is negative
1$:	TST (R3)+	;Fix up stack - pop base
	BR SRET		;All done - return
2$:	LDF @(R3),AC0	;AC0 ← exponent
	STCFI AC0,R0	;R0 ← INT(exponent)
	LDCIF R0,AC1	;AC1 ← INT(exponent)
	CMPF AC0,AC1	;Is exponent an integer?
	CFCC
	BNE 3$		; Nope - use Exp(B*Log(A)) to compute A↑B
	BIT #1,R0	;Check if integer exponent is odd
	BEQ 3$		; Nope - skip ahead
	JSR PC,3$	; Yup - raise to power
	NEGF @(R3)	;	then negate result
	RTS PC		;All done
3$:	LDF @2(R3),AC0	;Pick up base
	JSR PC,@LLOG	;AC0 ← Log(A)
	MULF @(R3)+,AC0	;AC0 ← B * Log(A) = Log(A↑B)
	TST (R3)+	;Fix up stack - pop base
	JSR PC,@LEXP	;AC0 ← Exp(Log(A↑B)) = A↑B
	BR SRET		; & Return
DATA
INF:	.WORD	077777, 177777	;Largest possible positive floating point number
CODE
;		 VDOT, VMAG, SSBRTN

;96 -- 116 microseconds
VDOT:	;Scalar ← Vector dot Vector
	;S ← (X1X2 + Y1Y2 + Z1Z2) / W1W2
	PUSH <R2>	;Save R2.
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg 2]
	MOV (R3)+,R0	;R0 ← LOC[arg 1]
	CLRF AC0	;AC0 ← 0.  Running total
	MOV #3,R2	;R2 ← 3:  Length of vector
1$:	LDF (R0)+,AC1	;Form sum of products of first 3 terms
	MULF (R1)+,AC1	;
	ADDF AC1,AC0	;
	SOB R2,1$	;Loop until all 3 fields done.
	DIVF (R0),AC0	;Divide by W1
	DIVF (R1),AC0	;Divide by W2.  AC0 now has answer.
	JSR PC,YESCMP	;OK to compact now
	POP <R2>	;Restore R2
	BR SRET		;Store result & return

;199 -- 207 microseconds
VMAGN:	;Scalar ← Norm (vector)
	;S ← SQRT(XX + YY+ ZZ) / W
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg]
	LDF (R1)+,AC0	;AC0 ← X
	MULF AC0,AC0	;AC0 ← XX
	LDF (R1)+,AC1	;AC1 ← Y
	MULF AC1,AC1	;AC1 ← YY
	ADDF AC1,AC0	;AC0 ← XX + YY
	LDF (R1)+,AC1	;AC1 ← Z
	MULF AC1,AC1	;AC1 ← ZZ
	ADDF AC1,AC0	;AC0 ← XX + YY + ZZ
	MOV R1,-(SP)	;Push LOC[W] onto system stack, to save across SQRTF
	JSR PC,@LSQRTF	;AC0 ← SQRT(XX + YY + ZZ)
	DIVF @(SP)+,AC0	;AC0 ← AC0 / W
	JSR PC,YESCMP	;OK to compact now
	BR SRET		;Store result & return

SSBRTN:	;Call a routine.
	LDF @(R3)+,AC0	;AC0 ← arg
	FETCH R0	;R0 ← which routine (a small number)
	ASL R0		;Double (words → bytes)
	BLE 1$		;Too small.
	CMP R0,#SBLSIZ	;Too large?
	BGE 1$		;Yes
	JSR PC,@SBRLST(R0)	;Call a routine.  AC0 ← answer.
	BR SRET		;Store result & return
1$:	ALERR SSBRMS	;Complain
	SCC		;Set condition code
	RTS PC		;Done
DATA
SSBRMS::ASCIE </NO SUCH SUBROUTINE/>

SBRLST:	;List of legal subroutines
	0		;Illegal
	SQRT		;#1
	SIN		;#2
	COS		;#3
	TAN		;#4
	ASIN		;#5
	ACOS		;#6
	ATAN2		;#7
	LOG		;#8
	EXP		;#9
	RTIME		;#10
SBLSIZ == .-SBRLST	;The size of the list (bytes)

CODE
SQRT:	JMP @LSQRTF	;Let it do the returning
SIN:	JMP @LSNCSD	;Let it do the returning
COS:	JSR PC,@LSNCSD
	STF AC1,AC0
	RTS PC
TAN:	JSR PC,@LSNCSD
	DIVF AC1,AC0	;Tan = Sin / Cos
	RTS PC
ASIN:	JMP @LASIN	;Let it do the returning
ACOS:	JMP @LACOS	;Let it do the returning
ATAN2:	LDF @(R3)+,AC1	;Get second argument for atan2(#1,#2)
	JMP @LATAN2
LOG:	JMP @LLOG	;Let it do the returning
EXP:	JMP @LEXP	;Let it do the returning
RTIME:	GETTIM		;Get current time from kernel
	CLRB 1(SP)	;So we don't lose the lower bits on conversion
	SETL		;Time is in long format
	LDCLF (SP)+,AC1	;AC1 ← time in msecs
	SETI		;Back to normal short integer format
	DIVF THOUS,AC1	;AC1 ← time in seconds
	SUBF AC0,AC1	;Get elapsed time
	STF AC1,AC0	;Store it in AC0
	RTS PC

;Vector utilities:  UNITV, CROSV

;281 -- 286 microseconds  
UNITV:	;Vector ← V / Norm(V)
	;S ← SQRT(XX + YY+ ZZ)
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3),R1	;R1 ← LOC[arg]
	LDF (R1)+,AC0	;AC0 ← X
	MULF AC0,AC0	;AC0 ← XX
	LDF (R1)+,AC1	;AC1 ← Y
	MULF AC1,AC1	;AC1 ← YY
	ADDF AC1,AC0	;AC0 ← XX + YY
	LDF (R1)+,AC1	;AC1 ← Z
	MULF AC1,AC1	;AC1 ← ZZ
	ADDF AC1,AC0	;AC0 ← XX + YY + ZZ
	JSR PC,@LSQRTF	;AC0 ← SQRT(XX + YY + ZZ)
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV 2(R3),R1	;R1 ← LOC[old vector]
	MOV #3,R2	;R2 ← count of fields
1$:	LDF (R1)+,AC1	;AC1 ← field of vector
	DIVF AC0,AC1	;divide by norm
	STF AC1,(R0)+	;Store result
	SOB R2,1$	;Loop until done
	MOV ONE,(R0)+	;Set W to 1
	CLR (R0)	;   (two words long)
	MOV (R3)+,(R3)	;Fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

;172 -- 184 microseconds  
CROSV:	;Vector ← Vector cross Vector
	;X ← Y1Z2 - Y2Z1
	;Y ← X2Z1 - X1Z2
	;Z ← X1Y2 - X2Y1
	;W ← W1W2
	;AC0, 1, 2, 3, 4, 5 are garbaged by this routine.
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV 2(R3),R2	;R2 ← LOC[arg 2]
	MOV 4(R3),R1	;R1 ← LOC[arg 1].  Must not pop R3 stack yet!
	LDF 14(R1),AC0	;AC0 ← W1
	MULF 14(R2),AC0	;AC0 ← W1W2
	STF AC0,14(R0)	;Store AC0 → W
	LDF 4(R1),AC0	;AC0 ← Y1
	LDF (R2),AC1	;AC1 ← X2
	LDF 4(R2),AC2	;AC2 ← Y2
	LDF (R1),AC3	;AC3 ← X1
	STF AC3,AC4	;AC4 ← X1
	STF AC0,AC5	;AC5 ← Y1
	MULF AC2,AC3	;AC3 ← X1Y2
	MULF AC1,AC0	;AC0 ← X2Y1
	SUBF AC0,AC3	;AC3 ← X1Y2 - X2Y1
	STF AC3,10(R0)	;Z ← AC3
	LDF 10(R2),AC0	;AC0 ← Z2
	LDF 10(R1),AC3	;AC3 ← Z1
	MULF AC4,AC0	;AC0 ← X1Z2
	MULF AC3,AC1	;AC1 ← X2Z1
	SUBF AC0,AC1	;AC1 ← X2Z1 - X1Z2
	STF AC1,4(R0)	;Y ← AC1
	LDF 10(R2),AC0	;AC0 ← Z2
	MULF AC5,AC0	;AC0 ← Y1Z2
	MULF AC2,AC3	;AC3 ← Y2Z1
	SUBF AC3,AC0	;AC0 ← Y1Z2 - Y2Z1
	STF AC0,(R0)	;X ← AC0
	MOV (R3)+,2(R3) ;Put result cell where first arg was 
	TST (R3)+	; & fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

;TRANS extraction routines: TPOS, TORIEN, TAXIS, TMAGN

TPOS:	;Extracts the position part of a TRANS (last column)
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC,GETVEC	;R0 ← -(R3) ← LOC[New Vector]
	MOV	2(R3),R1	;R1 ← LOC[TRANS]
	ADD	#44,R1		;R1 ← LOC [last column of TRANS]
	MOV	#6,R2		;Three 2-word components to move
1$:	MOV	(R1)+,(R0)+	;Copy it
	SOB	R2,1$
	MOV	ONE,(R0)+	;Stick in the scale factor
	CLR	(R0)
	MOV	(R3)+,(R3)	;Fix-up stack
	JSR 	PC,YESCMP	;OK to compact now
	CCC			;Clear condition codes
	RTS	PC		; & Return

TORIEN:	;Extracts the rotation part of a TRANS
	JSR 	PC,NOCMP	;Don't compact for a bit
	JSR	PC,GETTRN	;R0 ← -(R3) ← LOC[New TRANS]
	MOV	2(R3),R1	;R1 ← LOC[TRANS]
	MOV	#22,R2		;Three columns to do, three 2-word #'s/col
1$:	MOV	(R1)+,(R0)+	;Copy the ROTN
	SOB	R2,1$
	MOV	#6,R2
2$:	CLR	(R0)+		;Set up last column, three 0's
	SOB	R2,2$
	MOV	(R3)+,(R3)	;Fix-up stack
	JSR 	PC,YESCMP	;OK to compact now
	CCC			;Clear cond codes
	RTS	PC		; & Return

;TAXIS & TANGLE  routines to extract the axis vector and angle of rotation
;		 given a rotation (trans);

;Define some constants

DATA
ONE:	.FLT2	1.0
TWO:	.FLT2	2.0
CTHIRD:	.FLT2	0.576		;Square root of 1/3
C1001:	.FLT2	1.0001
C0001:	.FLT2	0.0001
CODE

TAXIS:	JSR	PC,TAXAN	;Get vector components in AC3,AC4 & AC5
	TST	(R3)+		;Fix stack
	JSR	PC,GETVEC	;Get a new vector to store results
	STF	AC3,(R0)+
	LDF	AC4,AC0
	STF	AC0,(R0)+	;Store X,Y & Z components
	LDF	AC5,AC0
	STF	AC0,(R0)+
	MOV	ONE,(R0)+	;Store scale factor of 1
	CLR	(R0)
	JSR 	PC,YESCMP	;OK to compact now
	CCC			;Clear condition codes
	RTS	PC		; & Return

TMAGN:	JSR	PC,TAXAN	;Get COS(angle) in AC0, vector components in AC 3-5
	STF	AC3,-(SP)	;Store X component
	JSR	PC,@LACOS	;Compute angle in AC0
	LDF	(SP)+,AC3	;Retrieve X
	LDF	CTHIRD,AC1	;Square root of 1/3
	LDF	AC3,AC2		;Get X
	ABSF	AC2
	CMPF	AC2,AC1		;ABS(X)-SQRT(1/3)
	CFCC			;Copy FPP cond codes into CPU cond codes
	BLT	1$
	LDF	34(R2),AC1	;Get (2,3)
	SUBF	24(R2),AC1	;(2,3) - (3,2)
	MULF	AC3,AC1		;Get sign of SIN(angle)
	BR	4$
1$:	LDF	AC4,AC2		;Get Y
	ABSF	AC2
	CMPF	AC2,AC1		;ABS(Y)-SQRT(1/3)
	CFCC			;Copy FPP cond codes into CPU cond codes
	BLT	2$
	LDF	10(R2),AC1	;Get (3,1)
	SUBF	30(R2),AC1	;(3,1) - (1,3)
	MULF	AC4,AC1		;Get sign of SIN(angle)
	BR	4$
2$:	LDF	AC5,AC2		;Get Z
	ABSF	AC2
	CMPF	AC2,AC1		;ABS(Z)-SQRT(1/3)
	CFCC			;Copy FPP cond codes into CPU cond codes
	BLT	3$
	LDF	14(R2),AC1	;Get (1,2)
	SUBF	4(R2),AC1	;(1,2) - (2,1)
	MULF	AC5,AC1		;Get sign of SIN(angle)
	BR	4$
3$:	ALERR	TMAGMS		;Complain
	CLRF	AC0		;& return NILROT
4$:	CFCC
	BLT	5$
	NEGF	AC0		;If SIN(angle) > 0 then negate angle
5$:	TST	(R3)+		;Clean up stack
	JSR 	PC,YESCMP	;OK to compact now
	JSR	PC,GETSCA	;Get a scalar
	STF	AC0,@(R3)	;Store the angle of rotation
	CCC			;Clear condition codes
	RTS	PC		; & Return

DATA
TMAGMS::ASCIE	</ROTATION STRANGENESS/>
CODE

TAXAN:	;Code common to both TAXIS & TMAGN
	JSR 	PC,NOCMP	;Don't compact for a bit
	MOV	(R3),R2		;R2 points to the ROT
	LDF	(R2),AC0	;(1,1)
	ADDF	20(R2),AC0	;(2,2)
	ADDF	40(R2),AC0	;AC0 ← [(1,1)+(2,2)+(3,3)-1]/2 = COS(angle)
	SUBF	ONE,AC0
	STF	AC0,AC3		;we'll use this later
	DIVF	TWO,AC0
	STF	AC0,AC1		;Make a copy
	ABSF	AC1
	CMPF	C1001,AC1	;If ABS(COS(angle)) > 1.0001 return the NILROT
	CFCC
	BGT	1$		;Else go and compute the axis of rotation
	CLRF	AC0
	STF	AC0,AC3
	STF	AC0,AC4		;NILROT = 0 degrees about (0,0,1)
	LDF	ONE,AC1
	STF	AC1,AC5
	RTS	PC
1$:	STF	AC0,-(SP)	;Store COS(angle) away for later
	NEGF	AC3
	ADDF	TWO,AC3		;AC3 ← 3 - (1,1) - (2,2) - (3,3)
	LDF	ONE,AC0
	SUBF	(R2),AC0	;(1,1)
	SUBF	20(R2),AC0	;(2,2)
	ADDF	40(R2),AC0	;(3,3)
	DIVF	AC3,AC0		;AC0 ← Z↑2
	CMPF	C0001,AC0
	CFCC
	BLT	3$		;If Z > 0.0001 skip ahead
	CLRF	AC5		;Set Z ← 0
	LDF	ONE,AC0
	SUBF	(R2),AC0	;(1,1)
	ADDF	20(R2),AC0	;(2,2)
	SUBF	40(R2),AC0	;(3,3)
	DIVF	AC3,AC0		;AC0 ← Y↑2
	CMPF	C0001,AC0
	CFCC
	BLT	2$		;If Y > 0.0001 skip ahead
	CLRF	AC4		;Set Y ← 0
	LDF	ONE,AC3		;Set X ← 1
	BR	5$		;Skip to end
2$:	JSR	PC,@LSQRTF	;Get SQRT(Y↑2)
	STF	AC0,AC4
	LDF	AC5,AC2		;Clear this for later
	BR	4$		;Skip ahead to where X is computed
3$:	JSR	PC,@LSQRTF	;Get SQRT(Z↑2)
	STF	AC0,AC5
	LDF	ONE,AC2
	STF	AC2,AC3		;For later
	SUBF	(R2),AC2	;(1,1)
	LDF	14(R2),AC0	;(1,2)
	DIVF	AC2,AC0		;AC0 ← (1,2) / [ 1 - (1,1) ]
	LDF	10(R2),AC2	;(3,1)
	MULF	AC0,AC2
	ADDF	20(R2),AC2	;(3,2)
	MULF	4(R2),AC0	;(2,1)
	SUBF	AC0,AC3
	SUBF	20(R2),AC3	;(2,2)
	DIVF	AC3,AC2		;AC2 ← [(3,2)+(3,1)*(1,2)/[1-(1,1)] /
				;	[1-(2,2)-(2,1)*(1,2)/[1-(1,1)]
	MULF	AC5,AC2
	STF	AC2,AC4		;AC4 ← Y
	LDF	10(R2),AC2	;(3,1)
	MULF	AC5,AC2		;Z
4$:	LDF	4(R2),AC3	;(2,1)
	MULF	AC4,AC3		;Y
	ADDF	AC2,AC3
	LDF	ONE,AC1
	SUBF	(R2),AC1	;(1,1)
	DIVF	AC1,AC3		;AC3 ← [(2,1)*Y+(3,1)*Z] / [1-(1,1)] = X
5$:	LDF	(SP)+,AC0	;Retrieve the COS(angle)
	RTS	PC		; & Return to TAXIS or TMAGN
;Return vectors: SVMUL, VSDIV, TVMUL, VMAKE, VADD, VSUB

;83 -- 91 microseconds
SVMUL:	;Vector ← Scalar * Vector.  Interpreter routine
	;X ← S*X,  Y ← S*Y,  Z ← S*Z,  W ← W
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV 2(R3),R2	;R2 ← LOC[arg2]  (the vector)
	LDF @4(R3),AC0	;AC0 ← arg1 (the scalar)
	MOV #3,R1	;R1 ← 3:  How many fields to handle
1$:	LDF (R2)+,AC1	;AC1 ← next field of vector
	MULF AC0,AC1	;AC1 ← product
	STF AC1,(R0)+	;Store result
	SOB R1,1$	;Loop until all 3 fields done.
	MOV (R2)+,(R0)+	;Transfer W
	MOV (R2)+,(R0)+	;  which is 2 words long.
	MOV (R3)+,2(R3)	;Fix-up stack
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VSDIV:	;Vector ← Vector / Scalar.  Interpreter routine
	;X ← X/S,  Y ← Y/S,  Z ← Z/S,  W ← W
	JSR PC,NOCMP	;Don't compact for a bit
	LDF @(R3)+,AC0	;AC0 ← arg2 (the scalar)
	MOV (R3),R2	;R2 ← LOC[arg1]  (the vector)
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector block]
	MOV #3,R1	;R1 ← 3:  How many fields to handle
1$:	LDF (R2)+,AC1	;AC1 ← next field of vector
	DIVF AC0,AC1	;AC1 ← product
	STF AC1,(R0)+	;Store result
	SOB R1,1$	;Loop until all 3 fields done.
	MOV (R2)+,(R0)+	;Transfer W
	MOV (R2)+,(R0)+	;  which is 2 words long.
	MOV (R3)+,(R3)	;Fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VMAKE:	;Interpreter routine
	LDF @(R3)+,AC3	;Fetch arg3 (Z)
	LDF @(R3)+,AC2	;Fetch arg2 (Y)
	LDF @(R3)+,AC1	;Fetch arg1 (X)
VMAKE0::		;entry point for POINTY
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	STF AC1,(R0)+	;Store X
	STF AC2,(R0)+	;Store Y
	STF AC3,(R0)+	;Store Z
	MOV ONE,(R0)+	;Store W
	CLR (R0)	;Store W (second word)
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VADD:	;Interpreter routine
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R0	;R0 ← LOC[arg 2] (a vector)
	MOV (R3)+,R1	;R1 ← LOC[arg 1] (a vector)
	LDF (R0)+,AC1	;Calculate X
	ADDF (R1)+,AC1
	LDF (R0)+,AC2	;Calculate Y
	ADDF (R1)+,AC2
	LDF (R0)+,AC3	;Calculate Z
	ADDF (R1)+,AC3
VRET:	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	STF AC1,(R0)+	;Store X
	STF AC2,(R0)+	;Store Y
	STF AC3,(R0)+	;Store Z
	MOV ONE,(R0)+	;Assume W is 1
	CLR (R0)
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VSUB:	;Interpreter routine
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3)+,R1	;R1 ← LOC[arg 2] (a vector)
	MOV (R3)+,R0	;R0 ← LOC[arg 1] (a vector)
	LDF (R0)+,AC1	;Calculate X
	SUBF (R1)+,AC1
	LDF (R0)+,AC2	;Calculate Y
	SUBF (R1)+,AC2
	LDF (R0)+,AC3	;Calculate Z
	SUBF (R1)+,AC3
	BR VRET		;Use common end code in VADD above

;283 -- 324 microseconds
TVMUL:	;Vector ← Trans * Vector.  Interpreter routine
	JSR PC,NOCMP	;Don't compact for a bit
	MOV (R3),R2	;R2 ← LOC[arg2] (the vector)
	MOV 2(R3),R0	;R0 ← LOC[arg1] (the trans)
	CLRF AC1	;X ← 0
	CLRF AC2	;Y ← 0
	CLRF AC3	;Z ← 0
	MOV #4,R1	;R1 ← How many columns left to go
1$:	LDF (R2)+,AC0	;AC0 ← field of vector
	STF AC0,AC5	;AC5 ← copy of AC0
	MULF (R0)+,AC0
	ADDF AC0,AC1	;Add partial result to X
	LDF AC5,AC0	;Restore AC0
	MULF (R0)+,AC0
	ADDF AC0,AC2	;Add partial result to Y
	LDF AC5,AC0	;Restore AC0
	MULF (R0)+,AC0
	ADDF AC0,AC3	;Add partial result to Z.
	SOB R1,1$	;Go back to do all 4 columns.
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	STF AC1,(R0)+	;Store X
	STF AC2,(R0)+	;Store Y
	STF AC3,(R0)+	;Store Z
	MOV -4(R2),(R0)+;Copy W from the vector
	MOV -2(R2),(R0)	;  (2 words long)
	MOV (R3)+,2(R3)	;Put result cell where first arg was
	TST (R3)+	; & fix up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done
;Return a trans: TMAKE, TVADD, TVSUB, TTMUL, TINVRT, VSAXWR, CONSTR

TMAKE:	;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and the vector from the second part;
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	MOV 4(R3),R2	;R2 ← LOC[arg 1] (the trans)
	MOV #11,R1	;R1 ← Count of how many copies to make
1$:	MOV (R2)+,(R0)+	;Transfer first half of floating word
	MOV (R2)+,(R0)+	;Transfer second half of floating word
	SOB R1,1$	;Repeat until done
	MOV 2(R3),R2	;R2 ← LOC[arg 2] (the vector)
	MOV #3,R1	;R1 ← Count of how many copies to make
2$:	MOV (R2)+,(R0)+	;Transfer first half of floating word
	MOV (R2)+,(R0)+ ;Transfer second half of floating word
	SOB R1,2$	;Repeat until done
	MOV (R3)+,2(R3)	;Fix-up stack
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code.
	RTS PC		;Done.

TVCOM:	;Utility routine used to do common code in TVADD & TVSUB
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	MOV 2(R3),R2	;R2 ← LOC[arg 2] (the vector)
	MOV 4(R3),R1	;R1 ← LOC[arg 1] (the trans)
	MOV #11,R3	;R3 ← Count of how many copies to make
1$:	MOV (R1)+,(R0)+	;Transfer first half of floating word
	MOV (R1)+,(R0)+	;Transfer second half of floating word
	SOB R3,1$	;Repeat until done
	MOV #3,R3	;R3 ← Count of how many additions to perform
	RTS PC		;Return to TVADD or TVSUB

TVADD:	;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and add the vector from the first part to the second argument.
	PUSH <R3>	;Save R3
	JSR PC,TVCOM	;Do the common code for TVADD & TVSUB
1$:	LDF (R1)+,AC0	;AC0 ← word from trans
	ADDF (R2)+,AC0	;  + word from vector
	STF AC0,(R0)+	;
	SOB R3,1$	;Repeat until done
TVRET:	POP <R3>	;Restore R3
	MOV -2(R3),2(R3)	;Fix-up stack (pretty strange huh?)
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code.
	RTS PC		;Done.

TVSUB:	;Interpreter routine.
;All that is required is to take the rot part of the first argument,
;and subtract the second argument from the vector of the first arg.
	PUSH <R3>	;Save R3
	JSR PC,TVCOM	;Do the common code for TVADD & TVSUB
1$:	LDF (R1)+,AC0	;AC0 ← word from trans
	SUBF (R2)+,AC0	;  + word from vector
	STF AC0,(R0)+	;
	SOB R3,1$	;Repeat until done
	BR  TVRET	;Do common end code & return

TTMUL:	;Interpreter routine
;Multiplies two transes together.
	PUSH <R4>	;Save R4
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	MOV 2(R3),R2	;R2 ← LOC[arg 2]
	MOV 4(R3),R4	;R4 ← LOC[arg 1]
	PUSH <R3,R4>	;Save R3 & a copy of R4
	MOV #4,R1	;Loop count for cols of answer
1$:	LDF (R2)+,AC1	;Pick up a column of arg2: First row
	LDF (R2)+,AC2	;  Second row
	LDF (R2)+,AC3	;  Third row
	STF AC3,AC4	;    store in AC4
	MOV #3,R3	;Loop count for rows of answer
2$:	LDF (R4),AC3	;First col of arg 1
	MULF AC1,AC3
	LDF 14(R4),AC0	;Second col of arg 1
	MULF AC2,AC0
	ADDF AC0,AC3
	LDF 30(R4),AC0	;Third col of arg 1
	MULF AC4,AC0	;
	ADDF AC0,AC3	;
	STF AC3,(R0)+	;
	ADD #4,R4	;Move to the next column of arg 1
	SOB R3,2$	;Repeat for first 3 rows of answer
	MOV (SP),R4	;Reset R4 to point to first row of arg 1
	SOB R1,1$	;Repeat for all four columns of answer
	LDF -14(R0),AC1	;Add correction for last column, first row
	ADDF 44(R4),AC1
	STF AC1,-14(R0)
	LDF -10(R0),AC1	;Add correction for last column, second row
	ADDF 50(R4),AC1
	STF AC1,-10(R0)
	LDF -4(R0),AC1	;Add correction for last column, third row
	ADDF 54(R4),AC1
	STF AC1,-4(R0)
	TST (SP)+	;Pop the R4 temp
	POP <R3,R4>	;Restore R3 & R4
	MOV (R3)+,2(R3)	;Fix-up stack
	TST (R3)+
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

TINVRT:	;Interpreter routine
COMMENT ⊗ Inverts a trans.
	  The result, (rot',trslat'), is defined:
					rot' = transpose(rot)
					trslat' = -(rot'*trslat)
⊗
	JSR PC,NOCMP	;Don't compact for a bit
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans] + 4*interation number
	MOV 2(R3),R2	;R2 ← LOC[old trans], travels down the whole trans
	PUSH <R3,R4>	;Save R3 & R4
	MOV R0,R3	;R3 ← LOC[new trans] + 20*interation number
	MOV R2,R4	;R4 ← LOC[old trans], stays constant
	MOV #3,R1	;Three columns to do
1$:	;Transpose a column, multiplying by the translation
	CLRF AC1	;Cumulative product
	LDF (R2)+,AC0	;Take from the source rotation
	STF AC0,(R0)	;  into the transpose,
	MULF 44(R4),AC0
	SUBF AC0,AC1	;accumulate the product.
	LDF (R2)+,AC0	;Take from the source rotation
	STF AC0,14(R0)	;  into the transpose,
	MULF 50(R4),AC0
	SUBF AC0,AC1	;accumulate the product.
	LDF (R2)+,AC0	;Take from the source rotation
	STF AC0,30(R0)	;  into the transpose
	MULF 54(R4),AC0
	SUBF AC0,AC1	;accumulate the product
	STF AC1,44(R0)	;Place the new translation
	ADD #4,R0	;Move to next row of result
	ADD #14,R3	;Move to next column of result
	SOB R1,1$
	POP <R4,R3>	;Restore R4 & R3
	MOV (R3)+,(R3)	;Fix-up stack
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

VSAXWR:	;Interpreter Routine	coded by ARG 5/3/76
;Produces a trans that rotates about a vector by a given angle
	PUSH <R5>		;Save R5
	LDF @(R3)+,AC2		;Save angle in AC2
	JSR PC,UNITV		;Convert vector to unit vector
	STF AC2,AC0		;Retrieve angle
	JSR PC,@LSNCSD		;Get sin & cos of angle
	STF AC0,AC4		;Save sin in AC4
	STF AC1,AC5		;Save cos in AC5
	SUBF ONE,AC1		;AC1←(1-COS)
	NEGF AC1
	JSR PC,NOCMP		;Don't compact for a bit
	JSR PC,GETTRN		;R0←-(R3)←LOC[New Tran]
	MOV 2(R3),R1		;R1←LOC[Unit Vec]
	PUSH <#3>		;Three columns to do
1$:	MOV #3,R5		;Three rows to do
	MOV 2(R3),R2		;R2←LOC[Unit vec]
	LDF AC1,AC2
	MULF (R1)+,AC2		;AC2←(1-COS)*U[i]
2$:	LDF AC2,AC3
	MULF (R2)+,AC3		;Trans[j,i]←(1-COS)*U[i]*U[j]
	STF AC3,(R0)+
	SOB R5,2$		;Do all 3 rows
	DEC (SP)
	BGT 1$			;Do all 3 columns
	POP <(R0)+>
	CLR (R0)+		;Set up last column
	CLR (R0)+
	CLR (R0)+
	CLR (R0)+
	CLR (R0)+
	MOV #3,R5		;Three terms to do: (1,1) (2,2) & (3,3)
	MOV (R3),R0		;R0←LOC[Trans]
3$:	LDF AC5,AC1		;AC1←COS
	ADDF (R0),AC1		;Add COS to (1-COS)*U[i]*U[i] term
	STF AC1,(R0)
	ADD #20,R0		;R0 points to next term to add COS to
	SOB R5,3$		;Do all three terms
	MOV (R3),R0		;R0←LOC[Trans]
	MOV 2(R3),R1		;R1←LOC[Unit Vec]
	LDF (R1)+,AC2		;AC2←U[X]
	MULF AC4,AC2		;AC2←SIN*U[X]
	STF AC2,AC3		;Make a copy
	ADDF 24(R0),AC2		;Add it to the (3,2) term
	STF AC2,24(R0)
	NEGF AC3
	ADDF 34(R0),AC3		;Subtract it from the (2,3) term
	STF AC3,34(R0)
	LDF (R1)+,AC2		;AC2←U[Y]
	MULF AC4,AC2		;AC2←SIN*U[Y]
	STF AC2,AC3		;Make a copy
	ADDF 30(R0),AC2		;Add it to the (1,3) term
	STF AC2,30(R0)
	NEGF AC3
	ADDF 10(R0),AC3		;Subtract it from the (3,1) term
	STF AC3,10(R0)
	LDF (R1)+,AC2		;AC2←U[Z]
	MULF AC4,AC2		;AC2←SIN*U[Z]
	STF AC2,AC3		;Make a copy
	ADDF 4(R0),AC2		;Add it to the (2,1) term
	STF AC2,4(R0)
	NEGF AC3
	ADDF 14(R0),AC3		;Subtract it from the (1,2) term
	STF AC3,14(R0)		;Trans is done!
	POP <R5>		;Restore R5
	MOV (R3)+,(R3)		;Clean up stack
	JSR PC,YESCMP		;OK to compact now
	CCC			;Clear condition codes
	RTS PC			; & Return

CONSTR:	;Interpreter routine
	JSR PC,NOCMP		;Don't compact for a while
	MOV 4(R3),-(R3)		;Copy origin
	JSR PC,VSUB		;Compute Vxy = V'xy - Vorg
	MOV 2(R3),-(R3)		;Copy V'x
	MOV 6(R3),-(R3)		;Copy origin
	JSR PC,VSUB		;Compute Vx = V'x - Vorg
	JSR PC,UNITV		;Make sure it's a unit vector
	MOV (R3)+,2(R3)		;Keep a copy for later
	MOV (R3),-(R3)		;Fix up stack for cross vector routine
	MOV 4(R3),2(R3)		; with Vxy on top & Vx beneath it
	JSR PC,CROSV		;Compute Vz = Vx ⊗ Vxy
	JSR PC,UNITV		;Make it a unit vector
	JSR PC,GETTRN		;R0 ← -(R3) ← LOC[New trans]
	MOV 4(R3),R1		;R1 ← LOC[Vx]
	MOV #6,R2
1$:	MOV (R1)+,(R0)+		;Copy Vx into first column of trans
	SOB R2,1$
	ADD #14,R0		;R0 ← LOC[3rd column of trans]
	MOV 2(R3),R1		;R1 ← LOC[Vz]
	MOV #6,R2
2$:	MOV (R1)+,(R0)+		;Copy Vz into third column of trans
	SOB R2,2$
	MOV 6(R3),R1		;R1 ← LOC[Vorg]
	MOV #6,R2
3$:	MOV (R1)+,(R0)+		;Copy Vorg into last column of trans
	SOB R2,3$
	MOV (R3)+,4(R3)		;Move LOC[trans] to bottom of stack
	MOV (R3)+,R1		;Fix up stack for cross vector routine
	MOV (R3),-(R3)		; with Vx on top & Vz beneath it
	MOV R1,2(R3)
	JSR PC,CROSV		;Compute Vy = Vz ⊗ Vx
	MOV (R3)+,R1		;R1 ← LOC[Vy]
	MOV (R3),R0		;R0 ← LOC[trans]
	ADD #14,R0		;R0 ← LOC[2nd column of trans]
	MOV #6,R2
4$:	MOV (R1)+,(R0)+		;Copy Vy into second column of trans
	SOB R2,4$
	JSR PC,YESCMP		;OK to compact now
	CCC			;Clear condition codes
	RTS PC			; & Return

;Affixment: AFFIX

AFFIX:	;Interpreter routine

COMMENT ⊗ This routine affixes two frames together. If necessary a frame header
will be created. An explicit trans may be given. The transes value is either
explicitly given on the stack, or created using the current values of the two
frames. Before doing the affixment a check is made that the frames are not
already affixed, and, if so, they are unfixed first. Frames which are affixed
to either arm are marked as being dynamic so they will be treated specially.⊗

	FETCH R0		;Get first frame offset
	JSR PC,GETARG		;R0 ← LOC[environment entry]
	BIT #HDRTYP,(R0)	;Test access type
	BNE 1$
	JSR PC,MFRAME		;If necessary make a new frame header
1$:	MOV 2(R0),R2		;R2 ← LOC[first frame header]
	FETCH R0		;Get second frame offset
	JSR PC,GETARG		;R0 ← LOC[environment entry]
	BIT #HDRTYP,(R0)	;Test access type
	BNE 2$
	JSR PC,MFRAME		;If necessary make a new frame header
2$:	MOV 2(R0),R1		;R1 ← LOC[second frame header]
AFFIX0:	PUSH <R2,R1>		;Save LOC[frame headers]; entry point for pointy
	JSR PC,UNFIX0		;Unfix them if affixed & get their values
	MOV #AFXSIZ,R0
	JSR PC,GTFREE		;R0 ← LOC[new affixment node]
	POP <R1,R2>		;Restore LOC[frame headers]
	EVWAIT GNEVT		;Enter critical section
	MOV CALCS(R2),(R0)	;Link into first frame's calculator list
	MOV R0,CALCS(R2)
	MOV R1,OTHER(R0)	;Point to who first is affixed to
	MOV CALCS(R1),NEXT2(R0)	;Link into second frame's calculator list
	MOV R0,CALCS(R1)
	ADD #10,CALCS(R1)	;Make it point to the right place
	MOV R2,US(R0)		;Point to who second is affixed to
	EVSIG GNEVT		;End critical section
	FETCH -(SP)		;Get the type info for the affixment
	BIT #EXPTRN,(SP)	;Is an explicit trans specified?
	BEQ 3$			;  nope
	PUSH <R2,R1,R0>		;  yes
	FETCH R0		;Get trans offset
	JSR PC,GETARG		;R0 ← LOC[environment entry for trans]
	TST (R0)+		;R0 ← LOC[value pointer for trans]
	POP <R1>		;R1 ← LOC[affixment node]
	MOV R0,TRANS(R1)	;Fill in trans pointer
	MOV R1,R0
	POP <R1,R2>		;Restore regs
3$:	TST (SP)		;See if trans value is already on stack
	BPL 6$			;  yup
	BIC #100000,(SP)	;  nope - first clear implicit trans value bit
	PUSH <R0,R2>		;    & then calculate the trans
	JSR PC,NOGC
	CALL GETVAL,<R1>	;Get value of second frame
	MOV R0,-(R3)		;Push value onto stack
	BNE 4$
	MOV #NILTRN,(R3)	;If none use NILTRAN as a default
4$:	JSR PC,TINVRT
	POP <R2>
	CALL GETVAL,<R2>	;Get value of first frame
	MOV R0,-(R3)		;Push value onto stack
	BNE 5$
	MOV #NILTRN,(R3)	;If none use NILTRAN as a default
5$:	JSR PC,TTMUL		;Trans ← (first) * inv(second)
	JSR PC,YESGC
	POP <R0>
6$:	BIS #AFXTYP,(SP)	;Mark the type as an affixment
	MOV (SP),TYPE(R0)	;Set up type bits
	MOV (SP),TYPE2(R0)
.IFNZ CPOINTY
	MOV US(R0),R2		;Make sure R2 points to first frame
	MOV #-1,INVMRK(R2)	;invalidate first frame
.ENDC
	BIS #FRAME2,TYPE2(R0)
	BIT #EXPTRN,(SP)+	;Now store away the trans value
	BEQ 7$
	MOV (R3)+,@TRANS(R0)	;  use explicit trans
	BR 8$
7$:	MOV (R3)+,TRANS(R0)	;  use implicit trans
8$:	MOV US(R0),R1		;Is either a device or affixed to a device?
	MOV OTHER(R0),R2	;R1 ← first frame, R2 ← second frame
	BIT #FTYPE,TYPE(R1)	;See if first is a device
	BNE 20$			; No - try second
	BIT #FTYPE,TYPE(R2)	;Second better not also be a device
	BNE 21$			;It's not - skip ahead and test if second is dynamic
	BR 22$			;Trying to affix two devices - go complain
20$:	BIT #FTYPE,TYPE(R2)	;See if second is a device
	BNE 24$			;No - skip ahead
	MOV R1,R0		;Swap R1 & R2
	MOV R2,R1
	MOV R0,R2
21$:	BIT #DYNAM,TYPE(R2)	;See if 2nd frame is already affixed to a device
	BEQ 23$			; No - skip ahead
	CMPB MECH(R1),DCNT+1(R2) ;If so had better be this device
	BEQ 30$			;It is same device, so all done
22$:	ALERR AFXERR		;It's a different device! Complain.
	BR 30$			; & then just punt
23$:	MOV MECH(R1),R1		;Get mech bits in R1
	SWAB R1			;R1 now has proper DCNT format: device,,cnt
	BR 27$			;Go mark second frame
24$:	BIT #DYNAM,TYPE(R1)	;Neither frame was a device - see if 1st is dynamic
	BNE 25$			; Yes - skip ahead
	BIT #DYNAM,TYPE(R2)	;See if second is dynamic
	BEQ 30$			; Nope - neither's dynamic so all done
	MOV R1,R0		;Swap R1 & R2
	MOV R2,R1
	MOV R0,R2
	BR 26$			;Go and mark first frame
25$:	BIT #DYNAM,TYPE(R2)	;See if 2nd frame is already affixed to a device
	BEQ 26$			; No - skip ahead and mark it
	CMPB DCNT+1(R1),DCNT+1(R2) ;If so had better be this device
	BEQ 30$			;It is same device, so all done
	BR 22$			;It's not the same device - go complain
26$:	MOV DCNT(R1),R1		;Get DCNT in R1
27$:	INC R1			; & update the count
	JSR PC,AFXAUX		;Finally, mark the frame as dynamic
30$:	CCC
	RTS PC			;Done

AFXAUX:	;Auxillary routine used by AFFIX to run down the affixment chain of
	;a frame that has just become dynamic. R2 points to the frame, R1 has
	;the dev,,cnt info, R0 is used to run through the list.

	BIT #FTYPE,TYPE(R2)	;See that we've got a frame
	BNE 1$			; Yup - skip ahead
	MOV MECH(R2),R1		;Get mech bits in R1
	SWAB R1			;R1 now has proper DCNT format: device,,cnt
	BR 2$			;Go mark other frames
1$:	BIT #DYNAM,TYPE(R2)	;See if we've been marked as dynamic yet
	BNE 4$			; Yup - all done here
	BIS #DYNAM,TYPE(R2)	;Mark us as being dynamic
	MOV R1,DCNT(R2)		;Indicate device we're affixed to & counter
2$:	INC R1			;Increment depth counter
	MOV CALCS(R2),R0	;R0 points to affixment chain to try marking
	BEQ 4$			; if any
3$:	PUSH <R1,R0>		;Save regs
	MOV OTHER(R0),R2	;R2 points to frame we're affixed to
	JSR PC,AFXAUX		;Try to mark him
	POP <R0,R1>
	MOV (R0),R0		;Run down affixment list
	BNE 3$
4$:	RTS PC			;All done

DATA
AFXERR::ASCIE </Can't have affixment chain connecting two arms!/>
CODE

;	   UNFIX

UNFIX:	;Interpreter routine

COMMENT ⊗ This routine unfixes two frames. Before unfixing, an attempt is made
to validate both frames. ⊗

	FETCH R0		;Get first frame offset
	JSR PC,GETARG		;R0 ← LOC[environment entry]
	BIT #HDRTYP,(R0)	;Check header exists
	BEQ UNRET		;  if not quit
	MOV 2(R0),R2		;R2 ← LOC[first frame header]
	FETCH R0		;Get second frame offset
	JSR PC,GETARG		;R0 ← LOC[environment entry]
	BIT #HDRTYP,(R0)	;Check header exists
	BEQ UNRET		;  if not quit
	MOV 2(R0),R1		;R1 ← LOC[second frame header]
UNFIX0:				;Entry point from AFFIX
	PUSH <R1,R2>
	BIT #FTYPE,TYPE(R1)	;Try to validate both frames before we unfix them
	BEQ 10$			;  Unless they're devices
	CALL GETVAL,<R1>
10$:	MOV (SP),R1
	BIT #FTYPE,TYPE(R1)
	BEQ 11$
	CALL GETVAL,<R1>
11$:	MOV (SP),R2		;Restore R2 & R1, but leave pointers on stack
	MOV 2(SP),R1
	EVWAIT GNEVT		;Enter critical region
	ADD #CALCS,R1		;R1 ← LOC[beginning of second's calculator list]
1$:	MOV (R1),R0		;R0 ← LOC[next calc to check]
	BEQ 2$			; if any
	BIT #AFXTYP,TYPE(R0)	;Make sure it's an affixment
	BEQ 2$
	CMP R2,OTHER(R0)	;See if affixed to first frame
	BEQ 3$			;  yes - found it
2$:	MOV (R1),R1		;Check next
	BNE 1$			;  if any
	CMP (SP)+,(SP)+		;Clear R1 & R2 off of stack
	BR 30$			;Whoops - wasn't there so split
3$:	MOV (R0),(R1)		;Remove us from second's calc list
	BIT #FRAME2,TYPE(R0)
	BNE 4$			;Treat the second frame slightly differently
	PUSH <R0>		;Save LOC[affixment node]
	ADD #10,R0		;R0 ← LOC[node as seen by other frame]
	BR 5$
4$:	SUB #10,R0
	PUSH <R0>
5$:	ADD #CALCS,R2		;R2 ← LOC[beginning of first's calculator list]
6$:	CMP (R2),R0		;Find affixment node in the list
	BEQ 7$
	MOV (R2),R2		;Check next node
	BNE 6$
	BR 8$			;Wasn't there!
7$:	MOV (R0),(R2)		;Unlink node from list
8$:	POP <R0>		;R0 ← LOC[affixment node]
	JSR PC,RLFREE		;Release it
	POP <R2,R1>		;Restore pointers to frame headers
	BIT #FTYPE,TYPE(R1)	;Is first a device?
	BEQ 21$			; Yes - unmark second
	BIT #FTYPE,TYPE(R2)	;Is second a device?
	BEQ 20$			; Yes - go unmark first frame
	BIT #DYNAM,TYPE(R1)	;Is first (and hence second) dynamic?
	BEQ 30$			; Nope - nothing to do so all done
	CMP DCNT(R1),DCNT(R2)	;See who's further away from device
	BEQ 30$			;If they're the same distance do nothing
	BLT 21$			;First is closer unmark second
20$:	MOV R1,R2		;Second is closer - unmark first frame
21$:	MOV DCNT(R2),R1		;DCNT to use to detect loops in affixment chain
	JSR PC,UFXAUX		;Go unmark frame pointed to by R2
30$:	EVSIG GNEVT		;End critical section
UNRET:	CCC
	RTS PC			;Done

UFXAUX:	;Auxillary routine used by UNFIX to run down the affixment chain of
	;a frame that has just ceased being dynamic. R2 points to the frame,
	;R1 has the dev,,cnt info, R0 is used to run through the list.

	BIT #FTYPE,TYPE(R2)	;Check that we've got a frame
	BEQ 1$			; No, it's a device! Need to remark.
	BIT #DYNAM,TYPE(R2)	;See if we've been unmarked as dynamic yet
	BEQ 4$			; Yup - all done here
	BIC #DYNAM,TYPE(R2)	;Mark us as no longer being dynamic
	CMP R1,DCNT(R2)		;Check that there isn't a loop in the affixment
	BLE 2$			; No loop finish unmarking frame
	MOV DCNT(R2),R1		; Yes there is a loop! Use this DCNT
1$:	JSR PC,AFXAUX		;  & Remark everyone
	CLR R1			;Indicate to whoever called us that we're finished
	BR 4$
2$:	CLR DCNT(R2)		;DCNT no longer counts
	MOV CALCS(R2),R0	;R0 points to affixment chain to try marking
	BEQ 4$			; if any
3$:	PUSH <R0>		;Save reg
	MOV OTHER(R0),R2	;R2 points to frame we're affixed to
	JSR PC,UFXAUX		;Try to unmark him
	POP <R0>
	TST R1			;See if DCNT is 0, means loop in affixment found
	BEQ 4$			; Yes - all done then
	MOV (R0),R0		;Run down affixment list
	BNE 2$
4$:	RTS PC			;All done

;Motion:  MOVE (start: code for finger motions)

NULLING==1		;Define some control bits for later use
WOBBLE==2
SPEEDF==4
DURREL==60
  DURLB==20	;lower bound on duration
  DURUB==40	;upper bound
  DUREB==60	;exact bound
VELOC==100
TCODE==200
VIAPT==400
DEPRPT==1000
APPRPT==2000
NODEPR==4000	;No departure point
DESTPT==10000

MOVE:	;Interpreter routine
	PUSH R5			;Give us another free register, we'll need it
	FETCH R0		;Get level-offset of control frame
MOVE0:				;Entry point for POINTY
	JSR PC,GETARG		;R0 ← Environment entry for control frame
	BIT #HDRTYP,(R0)+	;Should have a header
	BNE 2$			; It does handle below
	CMPB #TRNTYP,-2(R0)	;Is it at least a trans?
	BEQ 1$			; Yes - skip ahead
	PUNT NOTFRM		; Not even a trans!!! Gack!!! Give up.
1$:	ALWARN NODEVM		;No!!! Not a device or dynamic frame!!! Complain
	PUSH #BARM		;Assume barm
	MOV (R0),-(R3)		;Push value for computing trans offset
	JMP MVARM1		;Skip ahead

2$:	MOV (R0),R0		;R0 ← header
	BIT #FTYPE,TYPE(R0)	;See if it's a frame or a device
	BEQ 3$			; Device - skip ahead
	JMP MOVARM		; Handle frames below
3$:	MOV MECH(R0),CMECH(R4)	;Save mech bits in ISB
	PUSH R0
	MOV R0,R2		;R2 ← device header
	JSR PC,GETDEV		;Push device's value on R3 stack
	POP R0
	BIT #AHAND,MECH(R0)	;See if we're moving an arm or a hand
	BNE MOVHND		;Handle hands below
	CLR (R3)		;No trans offset needed to compute actual arm positions
	JMP MAKTRJ		;Skip ahead to make up rest of the trajectory

MOVHND:	BMPIPC			;Skip over # segments = 1
	BMPIPC			;Skip over control bits = destpt
	MOV #31,R0		;Coefficient lists for hands are easy
	JSR PC,GTFREE		;R0 ← Address of coefficient list
	PUSH R0			;Save it on stack
	BIT #YARM,CMECH(R4)	;See which hand we're moving
	BEQ 4$			;Deal with Blue hand below
	MOV #YHANDSB,(R0)	;Stick in proper servo bits
	LDF YJT7,AC3		;AC3 ← joint rate for Yellow hand
	BR 5$			;Skip ahead to common hand code
4$:	MOV #BHANDSB,(R0)	;Stick in proper servo bits
	LDF BJT7,AC3		;AC3 ← joint rate for Blue hand
5$:	LDF @(R3)+,AC1		;AC1 ← current hand opening
	JSR PC,MOVARG		;Get final position value
	LDF @(R3)+,AC0		;AC0 ← Final position
	CFCC
	BPL 6$			;Make sure it's positive
	CLRF AC0		;It is now
	BR 7$			;Go complain
6$:	CMPF MAXHD,AC0		;Now make sure not too big
	CFCC
	BPL 8$			;It's okay
	LDF MAXHD,AC0		;Make it ok
7$:	ALWARN NOSOLH		;Complain to user
8$:	MOV (SP),R0		;Restore pointer to coefficient list
	ADD #20,R0		;R0 ← coefficients
	STF AC1,(R0)+		;C0 = current position
	ADD #4,R0		;C1 = 0
	SUBF AC1,AC0		;AC0 ← final - current position (= ∂p)
	LDCIF #3,AC2		;AC2 ← 3.0
	MULF AC0,AC2		;AC2 ← 3 * ∂p
	STF AC2,(R0)+		;C2 = 3*∂p
	SUBF AC0,AC2		;AC2 ← 2 * ∂p
	NEGF AC2		;AC2 ← -2 * ∂p
	STF AC2,(R0)+		;C3 = -2*∂p
	ADD #14,R0		;C4 = C5 = Hand Gravity loading = 0.0
	LDF HANDIN,AC1		;Blue hand inertia = 1590000.0
	STF AC1,(R0)		;If Yellow hand is different need to change
	ABSF AC0		;AC0 ← |∂p|
	MULF AC0,AC3		;AC3 ← joint rate * |∂p| = time needed for motion
	FETCH R2		;R2 ← control bits
;	LDF @SPDFAC+2,AC2	;AC2 ← Global speed-factor - Not used for hands
	LDF ONE,AC2		;AC2 ← assume a speed-factor of 1.0 unless explicitly given one
	BIT #SPEEDF,R2		;Speed-factor given for this motion?
	BEQ 10$
	JSR PC,MOVARG		;Get speed-factor
	LDF @(R3)+,AC2		;AC2 ← Speed-factor
10$:	MULF AC3,AC2		;AC2 ← speed-factor * min-time
	MOV (SP),R0		;Restore pointer to coefficient list
	MOV R2,R1		;Copy control bits
	BIC #177776,R1		;Clear all but nulling bit (no-nulling = 1)
	MOV R1,4(R0)		;Set up command bits for servo
	ADD #10,R0		;R0 ← first (& only) segment
	MOV #50,(R0)+		;Rel seg pointer
	PUSH R0			;Save pointer to where duration will go
	BIT #DURREL,R2		;Explicit duration specified
	BEQ 15$			; No - skip ahead
	JSR PC,MOVARG		;Get duration value
	LDF @(R3)+,AC0		;AC0 ← Duration
	ABSF AC0		;Make sure it's positive
	BIT #DURUB,R2		;Upper or exact bound?
	BNE 12$			; Yes - skip ahead
	CMPF AC0,AC2		;Compare explicit time with computed time
	CFCC			;Lower bound - choose largest time
	BMI 15$			; Use computed time
	BR 14$			; Use explicit time
12$:	CMPF AC0,AC3		;Compare explicit time with minimum needed time
	CFCC			;Make sure user's allocated enough
	BPL 13$			; It's okay
	ALWARN TIMERR		; Not enough - use it, but complain
13$:	BIT #DURLB,R2		;Exact bound?
	BNE 14$			; Yes - use explicit time
	CMPF AC0,AC2		;Compare explicit time with computed time
	CFCC			;Upper bound - choose smallest time
	BPL 15$			; Use computed time
14$:	STF AC0,AC2		;Use user specified time
15$:	MULF THOUS,AC2		;Convert time to milliseconds
	STCFI AC2,@(SP)		;Specify how long motion should take
	ADD #300.,@(SP)+	;Add in a little slack time 0.3 seconds
	JMP MOVEND		;All done go call MOVSTA

;	 MOVE (continued: setup for arm motion)

MOVARM:	BIT #DYNAM,TYPE(R0)	;Better be a dynamic frame
	BNE 1$			; Yup - skip ahead
	ALWARN NODEVM		;Complain & assume barm
	PUSH #BARM
	BR 2$			;Skip ahead
1$:	MOVB DCNT+1(R0),CMECH(R4) ;Save mech bits in ISB
2$:	JSR PC,NOCMP		;Don't compact for a bit
	CALL GETVAL,<R0>	;R0 ← value of frame
	MOV R0,-(R3)		;Push value on interpreter stack.
	JSR PC,YESCMP		;OK to compact now
MVARM1:	JSR PC,TINVRT		;Invert it
	MOV #BARMHD,R2		;Now get the device value - assume barm
	BIT #BARM,CMECH(R4)	;Check mech bits
	BNE 3$			; Yup - it's the blue arm
	MOV #YARMHD,R2		; Nope - it's the yellow arm
3$:	JSR PC,GETDEV		;Put current value on R3 stack
	JSR PC,TTMUL		;Compute trans offset so we can find actual arm positions

MAKTRJ: JSR PC,NOGC		;Keep things simple
	FETCH R0		;Get number of segments
	INC R0			;Plus one more for destination
	MUL #100.,R0		;Per segment need 4wd header + 8flt/joint
	MOV R1,R0
	ADD #4,R0		;Add in 4 command words
	MOV R0,R2		;Make up a pointer to "extra" segment
	ASL R2
	ADD #130.,R0		;Need a little more room for computations
	JSR PC,GTFREE		;R0 ← Address of coefficient list
	ADD R0,R2		;R2 ← Address of "extra" segment
	PUSH <R0,#0>		;We'll be needing all these later
	MOV (R3)+,R1		;Now copy the offset trans - if any
	BEQ 2$
	ADD #2*76.,R2		;R2 ← Address of free space (inertia terms +)
	MOV R2,(SP)		;So we know that we've got a trans offset
	MOV #30,R0		;Size of trans (3 * 4 * 2 wds)
1$:	MOV (R1)+,(R2)+		;Copy trans
	SOB R0,1$
2$:	PUSH R2			;Save start of free space used by SOLVE & DTERMS
	MOV CMECH(R4),(R2)	;Save a copy of mech bits
	MOV 4(SP),R1		;R1 ← Address of coefficient list
	MOV LTHPTR,R5		;R5 ← pointer into current joint angles
	MOV #YARMSB,(R1)	;Set up servo bits for MOVE
	BIT #YARM,(R2)+		;See what mech we have
	BNE 3$			;If Yellow skip ahead
	ADD #14.,R5		;Correct pointer for Blue arm angles
	MOV #BARMSB,(R1)	;Set up servo bits for MOVE
3$:	ADD #20,R1		;R1 ← Address of actual coefficients
	MOV #6,R0		;Take care of all 6 joints
4$:	LDF @(R5)+,AC0		;Get current joint angle
	STF AC0,(R1)		; & store it in coefficient list
	MOV R1,(R2)+		;Make up pointer for later use by SOLVE & DTERMS
	MOV R1,12.(R2)		;One pointer list for each arm
	ADD #24.,R1		;Where next joint will go
	SOB R0,4$		;Do all 6
	ADD #16.,R2		;R2 ← start of table to inertia pointers
	MOV #6,R0		;6 joints to do
5$:	MOV R1,(R2)+		;Store pointer to gravity loading term
	ADD #4,R1
	MOV R1,10.(R2)		; & pointer to inertia term
	ADD #4,R1
	SOB R0,5$		;Repeat til done

MAKDEP:	BIT #DEPRPT,@(R4)	;See if we've got an explicit departure point
	BNE 11$			; Yes - handle it below
	MOV YDEPR+2,R0		;R0 ← Current departure point for Yellow arm
	BIT #YARM,CMECH(R4)	;Check which arm
	BNE 10$
	MOV BDEPR+2,R0		;R0 ← Current departure point for Blue arm
10$:	TST R0			;See if there is a departure point
	BEQ MAKVIA		; No - skip ahead to the VIAs
	MOV R0,-(R3)		;Push departure point onto R3 stack
	CLR R2			;Zero control bits
	BR 13$			;Go set up departure below
11$:	FETCH R2		;R2 ← Control bits for departure point
	BIT #NODEPR,R2		;Check that we want a departure point
	BNE MAKVIA		; Nope - move on to the VIA points
	JSR PC,MOVARG		;Get departure point value on R3 stack
	BIT #TCODE,R2		;Associated code?
	BEQ 12$			; No
	FETCH R0		;Get offset of event to signal
	JSR PC,GETARG		;R0 ← pointer into environment
	MOV (SP),R1		;R1 ← table of joint pointers
	MOV 2(R1),R1		;R1 ← Address of last seg's coefficients
	MOV 2(R0),-(R1)		;Store event address in previous segment
12$:	MOV 2(SP),R0		;R0 ← Address of offset trans (if any)
	BEQ 13$			;Skip ahead if none
	MOV R0,-(R3)		;Push it on stack
	JSR PC,TTMUL		;Compute arm position for departure point
13$:	MOV (SP),R2		;R2 ← table of joint pointers
	JSR PC,MOVPOS		;Set up C1 coefficients, differences & min time
	MOV (SP),R0		;R0 ← table of joint pointers
	MOV 2(R0),R1		;R1 ← Address of joint coefficients for this seg
	MOV #DEPRPT,-8.(R1)	;Remember that this is a departure point

MAKVIA:	FETCH R2		;Get control bits for next segment
	BIT #VIAPT,R2		;Was it a VIA
	BEQ MAKAPP		; No - see if it's an approach
	PUSH R2			;Save a copy of the control bits
	JSR PC,MOVARG		;Get via point value on R3 stack
	BIT #TCODE,R2		;Associated code?
	BEQ 1$			; No
	FETCH R0		;Get offset of event to signal
	JSR PC,GETARG		;R0 ← pointer into environment
	MOV 2(SP),R1		;R1 ← table of joint pointers
	MOV 2(R1),R1		;R1 ← Address of last seg's coefficients
	MOV 2(R0),-(R1)		;Store event address in previous segment
1$:	MOV 4(SP),R0		;R0 ← Address of offset trans (if any)
	BEQ 2$			;Skip ahead if none
	MOV R0,-(R3)		;Push it on stack
	JSR PC,TTMUL		;Compute arm position for via point
2$:	MOV 2(SP),R2		;R2 ← table of joint pointers
	JSR PC,MOVPOS		;Set up C1 coefficients, differences & min time
	MOV -2(R3),R5		;R5 ← Via position  (needed if velocity spec)
	POP R2			;Restore control bits
	MOV (SP),R0		;R0 ← table of joint pointers
	MOV 2(R0),R1		;R1 ← Address of joint coefficients for this seg
	MOV R2,-8.(R1)		;Remember if velocity or duration specifications
	BIT #DURREL,R2		;See if there's a duration spec
	BEQ 3$			; No
	JSR PC,MOVARG		;Get the duration spec
	LDF @(R3)+,AC0		;Copy it into the segment header
	MOV (SP),R0		;R0 ← table of joint pointers
	MOV 2(R0),R1		;R1 ← Address of joint coefficients for this seg
	STF AC0,-6(R1)
	BIT #DURUB,R2		;See if user time is less than minimum time needed
	BEQ 3$			;Skip ahead if lower bound was specified
	CMPF AC0,AC1		;Compare user specified time with min time (AC1)
	CFCC			;Make sure user's allocated enough
	BPL 3$			; It's okay
	ALWARN TIMERR		; Not enough - we'll use it, but complain
3$:	BIT #VELOC,R2		;See if velocity specified
	BEQ MAKVIA		; No - go on to next VIA point
	JSR PC,MOVARG		;Get velocity spec
	MOV (R3),R2		;R2 ← Velocity vector
	MOV R5,(R3)		;Push via position
	MOV #TWOTH,-(R3)	;Push 0.2
	MOV R2,-(R3)		;Push velocity vector
	JSR PC,SVMUL		;Offset = 0.2 * velocity
	JSR PC,TVADD		;Position + offset
	MOV (SP),R1
	TST (R1)+		;R1 ← table of joint pointers
	MOV #6,R0		;Change the pointers from C1 to C2
4$:	MOV (R1),R2		;R2 ← Address of coefficient
	LDF (R2)+,AC0		;Copy current joint angle
	STF AC0,(R2)
	MOV R2,(R1)+		;Modify both pointers
	MOV R2,12.(R1)
	SOB R0,4$
	MOV (SP),R1
	MOV (R1)+,R2		;R2 ← Mech bits, R1 ← table of joint pointers
	MOV (R3)+,R0		;R0 ← Via position + velocity offset
	JSR PC,@LSOLVE		;Compute joint angles for velocity offset
	MOV (SP),R1
	TST (R1)+		;R1 ← table of joint pointers
	MOV #6,R0		;Restore the pointers from C2 back to C1
	LDF FIVE,AC1		;AC1 ← 5.0
5$:	MOV (R1),R2		;R2 ← Address of coefficient C2
	LDF (R2),AC0		;AC0 ← velocity offset
	SUBF -(R2),AC0		;AC0 ← velocity offset - position
	MULF AC1,AC0		;AC0 ← 5 * (velocity offset - position)
	STF AC0,4.(R2)		;C2 ← joint velocity
	MOV R2,(R1)+		;Restore both pointers
	MOV R2,12.(R1)
	SOB R0,5$
	BR MAKVIA		;Go on to next VIA point

MAKAPP:	BIT #APPRPT,R2		;See if it's an Approach point
	BNE 12$			; Yes - handle below
	BIT #YARM,CMECH(R4)	;Check which arm
	BNE 11$			;Yellow - skip ahead
	CLR BAPPR+2		;Set approach point for Blue arm to NILDEPROACH
	BR MAKDST		;Go on to destination
11$:	CLR YAPPR+2		;Set approach point for Yellow arm to NILDEPROACH
	BR MAKDST		;Go on to destination
12$:	JSR PC,MOVARG		;Get approach point value on R3 stack
	BIT #TCODE,R2		;Associated code?
	BEQ 13$			; No
	FETCH R0		;Get offset of event to signal
	JSR PC,GETARG		;R0 ← pointer into environment
	MOV (SP),R1		;R1 ← table of joint pointers
	MOV 2(R1),R1		;R1 ← Address of last seg's coefficients
	MOV 2(R0),-(R1)		;Store event address in previous segment
13$:	MOV 2(SP),R0		;R0 ← Address of offset trans (if any)
	BEQ 14$			;Skip ahead if none
	MOV R0,-(R3)		;Push it on stack
	JSR PC,TTMUL		;Compute arm position for approach point
14$:	BIT #YARM,CMECH(R4)	;Check which arm
	BNE 15$			;Yellow - skip ahead
	MOV (R3),BAPPR+2	;Set new approach point for Blue arm
	BR 16$
15$:	MOV (R3),YAPPR+2	;Set new approach point for Yellow arm
16$:	MOV (SP),R2		;R2 ← table of joint pointers
	JSR PC,MOVPOS		;Set up C1 coefficients, differences & min time
	MOV (SP),R0		;R0 ← table of joint pointers
	MOV 2(R0),R1		;R1 ← Address of joint coefficients for this seg
	MOV #APPRPT,-8.(R1)	;Remember that this is an approach point
	
	FETCH R2		;R2 ← Control bits for destination
MAKDST:	PUSH R2			;Save it for later
	JSR PC,MOVARG		;Get destination value on R3 stack
	BIT #TCODE,R2		;Associated code?
	BEQ 1$			; No
	FETCH R0		;Get offset of event to signal
	JSR PC,GETARG		;R0 ← pointer into environment
	MOV 2(SP),R1		;R1 ← table of joint pointers
	MOV 2(R1),R1		;R1 ← Address of last seg's coefficients
	MOV 2(R0),-(R1)		;Store event address in previous segment
1$:	MOV 4(SP),R0		;R0 ← Address of offset trans (if any)
	BEQ 2$			;Skip ahead if none
	MOV R0,-(R3)		;Push it on stack
	JSR PC,TTMUL		;Compute arm position for destination point
2$:	MOV #BFINI,R0		;Save destination location for FINISH
	BIT CMECH(R4),#BARM	;Determine which arm we're using
	BNE 3$			; Blue
	MOV #YFINI,R0		; Yellow
3$:	MOV (R3),(R0)		;Copy destination value to BFINI or YFINI
	MOV 2(SP),R2		;R2 ← table of joint pointers
	JSR PC,MOVPOS		;Set up C1 coefficients, differences & min time
	POP R2			;Retrieve control bits
	MOV (SP),R0		;R0 ← table of joint pointers
	MOV 2(R0),R0		;R0 ← Address of this seg's coefficients
	MOV R2,-8.(R0)		;Store control bits in segment header

	JSR PC,YESGC		;Ok again
	ADD #4,SP		;All done with pointer tables & offset trans
	FETCH R2		;Get control bits for entire motion
	MOV (SP),R0		;R0 ← Coefficient list
	ADD #4,R0		;R0 ← Address command bits
	MOV R2,(R0)		;Store command bits: nulling, wobble
	BIC #177774,(R0)+
	BIT #WOBBLE,R2		;Is a wobble specified?
	BEQ MOVSPD		; No
	PUSH R0			;Save address of wobble pointer
	JSR PC,MOVARG		;Get the wobble value
	LDF @(R3)+,AC0		;Copy it into end of coeff list
	MOV 2(SP),R1		;R1 ← Start of coeff list
	SUB -(R1),R1		;R1 ← End of coeff list (added in block length)
	SUB #6,R1
	STF AC0,(R1)
	MOV R1,@(SP)+		;Set up wobble pointer

;	 MOVE (continued: speed-factor & time constraints)

MOVSPD:	LDF @SPDFAC+2,AC3	;AC3 ← Global speed-factor
	BIT #SPEEDF,R2		;Speed-factor given for this motion?
	BEQ 1$
	JSR PC,MOVARG		;Get local speed-factor
	LDF @(R3)+,AC3		;AC3 ← Speed-factor

1$:	CLRF AC0
	CMPF TWO,AC3		;For 1st/last segs add some slack time to start/stop
	CFCC			; If the speed-factor is less than 2.0
	BMI 2$			;Speed-factor is greater than 2.0 so don't bother
	LDF TWO,AC0		;AC0 ← 2.0
	SUBF AC3,AC0		;AC0 ← 2.0 - speed-factor
	MULF FIFTNH,AC0		;AC0 ← 0.15 * (2.0 - speed-factor)
2$:	STF AC0,AC4		;AC4 ← Slack time to add
	STF AC0,AC5		;Make a copy for later
	CLRF AC1		;AC1 will hold total time for motion
	CLRF AC2		;AC2 will hold total time for stretchable segments
	MOV (SP),R0		;R0 ← Start of coefficient list
	ADD #208.,R0		;R0 ← first segment
	CLR R1			;R1 ← last segment's control bits
	LDF 16.(R0),AC0		;AC0 ← min time stored by MOVPOS in C3
	BIT #DEPRPT,(R0)	;See if it's a departure point
	BEQ 5$			; No, skip ahead
3$:	CMPF TWO,AC3		;For deproaches make sure we use a
	CFCC			;  speed-factor ≥ 2
	BPL 4$
	MULF AC3,AC0		;AC0 ← Speed-factor * ST
	BR 6$			;Now go add it to ST & TT
4$:	ADDF AC0,AC0		;AC0 ← 2.0 * ST
	BR 6$			;Now go add it to ST & TT

5$:	BIT #APPRPT,R1		;Is it an approach segment?
	BNE 3$			; Yup - handle like departure above
	MULF AC3,AC0		;AC0 ← min seg time * speed-factor
	ADDF AC5,AC0		;Add some slack time if speed-factor < 2
	ADDF AC4,AC0		; & add a little more to first segment
	CLRF AC4		;   but only to first segment
	BIT #APPRPT,(R0)	;Is it an approach point?
	BEQ 6$			; No
	ADDF AC5,AC0		; Yes - add some more slack time for last seg
6$:	CMPF TWOTH,AC0		;Make sure each segment is at least 0.2 sec long
	CFCC
	BLE 7$			; Ok - segment long enough
	LDF TWOTH,AC0		; Now it is
7$:	BIT #DURREL,(R0)	;Any time constraints on this segment?
	BEQ 10$			; Nope all done here then
	BIT #DURUB,(R0)		;Upper or exact bound?
	BNE 9$			; Yes - skip ahead
	CMPF 2(R0),AC0		;Compare explicit time with computed time
	CFCC			;Lower bound - choose largest time
	BLT 11$			; Use computed time
8$:	LDF 2(R0),AC0		; Use explicit time
	BR 11$
9$:	BIT #DURLB,(R0)		;Exact bound?
	BNE 8$			; Yes - use explicit time
	CMPF 2(R0),AC0		;Compare explicit time with computed time
	CFCC			;Upper bound - choose smallest time
	BMI 8$			; Use explicit time
	BR 11$			;Go add seg time to total time

10$:	ADDF AC0,AC2		;ST ← ST + seg time	(stretchable time)
11$:	ADDF AC0,AC1		;TT ← TT + seg time	(total time)
	STF AC0,16.(R0)		;Store updated seg time back in C3
	MOV (R0),R1		;Save old control bits
	ADD #200.,R0		;R0 ← Next segment
	LDF 16.(R0),AC0		;AC0 ← min time stored by MOVPOS in C3
	BIT #DESTPT,R1		;See if more segments
	BEQ 5$			; Yup - repeat

MOVTIM:	BIT #DURREL,R2		;See if time constraint for entire motion
	BEQ 1$			; No - skip ahead to SEGTIM
	JSR PC,MOVARG		;Go get duration for motion
	LDF @(R3)+,AC0		;AC0 ← duration length
	CMPF AC0,AC1		;Compare explicit time with total time
	BIT #DURUB,R2		;Handle upper bound & exact below
	BNE 2$
	CFCC			;See if lower bound < total time
	BGT STRETCH		; Nope - need to stretch motion time (if we can)
1$:	JMP SEGTIM		; Yup - all done here
2$:	BIT #DURLB,R2		;Handle exact below
	BNE 3$
	CFCC			;See if upper bound > total time
	BGE 1$			; Yup - all done here, go to SEGTIM
	BR SHRINK		; Nope - need to shrink motion time (if we can)
3$:	CFCC			;See if exact bound < or > total time
	BLE SHRINK		; < - need to shrink motion time (if we can)

STRETCH: SUBF AC1,AC0		;AC0 ← UT - TT
	MOV (SP),R0		;R0 ← Coefficient list - to find lower bounded segs
	ADD #208.,R0		;R0 ← first segment
1$:	BIT #DURLB,(R0)		;Is time for this segment have a lower bound?
	BEQ 2$			; No
	BIT #DURUB,(R0)		;Make sure not exact
	BNE 2$			; It was
	ADDF 16.(R0),AC2	;This segment is stretchable, update ST
2$:	BIT #DESTPT,(R0)	;Any more?
	BNE 3$			; No
	ADD #200.,R0
	BR 1$			; Yes - do them

3$:	TSTF AC2		;Any stretchable segments?
	CFCC
	BNE 4$			; Yes
	ALWARN DURERR		; No - complain & ignore overall duration request
	BR SEGTIM

4$:	ADDF AC2,AC0		;AC0 ← (UT - (TT - ST)) / ST
	DIVF AC2,AC0
	MOV (SP),R0		;R0 ← Coefficient list - Let's stretch the segs
	ADD #208.,R0		;R0 ← first segment
5$:	BIT #DURUB,(R0)		;Does this segment have an upper or exact bound?
	BNE 6$			; Yes - leave it alone
	LDF 16.(R0),AC1		;C3 ← old time * stretch factor
	MULF AC0,AC1
	STF AC1,16.(R0)
6$:	BIT #DESTPT,(R0)	;Any more?
	BNE SEGTIM		; No
	ADD #200.,R0
	BR 5$			; Yes - do them

SHRINK:	MULF AC0,AC3		;Check that UT > (TT / Speed-factor)
	CMPF AC3,AC1		; i.e. that we have enough time for the motion
	CFCC			; really just a guess
	BPL 1$
	ALWARN TIMERR		;Complain if not enough
1$:	SUBF AC1,AC0		;AC0 ← UT - TT
	MOV (SP),R0		;R0 ← Coefficient list - to find upper bounded segs
	ADD #208.,R0		;R0 ← first segment
2$:	BIT #DURUB,(R0)		;Is time for this segment have an upper bound?
	BEQ 3$			; No
	BIT #DURLB,(R0)		;Make sure not exact
	BNE 3$			; It was
	ADDF 16.(R0),AC2	;This segment is shrinkable, update ST
3$:	BIT #DESTPT,(R0)	;Any more?
	BNE 4$			; No
	ADD #200.,R0
	BR 2$			; Yes - do them

4$:	TSTF AC2		;Any shrinkable segments?
	CFCC
	BNE 5$			; Yes
	ALWARN DURERR		; No - complain & ignore overall duration request
	BR SEGTIM

5$:	ADDF AC2,AC0		;AC0 ← (UT - (TT - ST)) / ST
	DIVF AC2,AC0
	MOV (SP),R0		;R0 ← Coefficient list - Let's shrink the segs
	ADD #208.,R0		;R0 ← first segment
6$:	BIT #DURLB,(R0)		;Does this segment have a lower or exact bound?
	BNE 7$			; Yes - leave it alone
	LDF 16.(R0),AC1		;C3 ← old time * stretch factor
	MULF AC0,AC1
	STF AC1,16.(R0)
7$:	BIT #DESTPT,(R0)	;Any more?
	BNE SEGTIM		; No
	ADD #200.,R0
	BR 6$			; Yes - do them

SEGTIM:	MOV (SP),R0		;R0 ← Coefficient list
	ADD #208.,R0		;R0 ← first segment
1$:	MOV R0,R1		;R1 ← first C3 coefficient
	ADD #16.,R1
	LDF (R1),AC0		;AC0 ← seg time
	STF AC0,AC1		;Copy it
	MULF THOUS,AC1		;Convert seconds to milliseconds
	STCFI AC1,-198.(R0)	;Convert to integer & store in previous seg header
	CLR -196.(R0)		;Zero out trans word
	BIT #DEPRPT,(R0)	;Don't change C4 for deproach segments
	BNE 3$			; Skip ahead if departure
	BIT #APPRPT,-200.(R0)
	BNE 3$			; Skip ahead if approach
	MOV #6,R2		;Set up C3 & C4 terms for all 6 joints
2$:	STF AC0,(R1)+		;C3 ← seg time
	LDF (R1),AC1		;AC1 ← delta position
	DIVF AC0,AC1		;C4 ← delta position / seg time
	STF AC1,(R1)
	ADD #20.,R1		;R1 ← C3 for next joint
	SOB R2,2$		;Do next joint
3$:	BIT #DESTPT,(R0)	;Was this the last segment?
	BNE MOVDEP		; Yes - finally ready to set up the polynomials
	ADD #200.,R0		;Go on to next segment
	BR 1$

;	 MOVE (concluded: handle deproach polys, call spline routine & end cleanup)

MOVDEP:	BIT #APPRPT,-200.(R0)	;Do we have an approach point?
	BEQ 2$			; No - check for departure
	BIS #VELOC,-200.(R0)	;Note that approach points have a velocity spec
	ADD #8.,R0		;R0 ← C1 for first joint
	LDF 8.(R0),AC0		;AC0 ← Seg time (C3)
	MOV #6,R1		;6 joints to do
1$:	LDF 12.(R0),AC1		;AC1 ← joint difference: final - approach = del
	STF AC1,16.(R0)		;C5 = del	(store it here temporarily)
	ADDF AC1,AC1		;AC1 ← 2 * del	(will move it back after we)
	STF AC1,4.(R0)		;C2 = 2 * del	(use previous seg's coeffs)
	NEGF AC1		;		(for the splining)
	STF AC1,12.(R0)		;C4 = -2 * del
	CLRF 8.(R0)		;C3 = 0.0
	NEGF AC1
	DIVF AC0,AC1		;AC1 ← 2 * del / seg time = velocity
	STF AC1,-196.(R0)	;C2(previous segment) = velocity
	ADD #24.,R0		;R0 ← C1 for next joint
	SOB R1,1$		;Do all 6 joints

2$:	MOV (SP),R0		;R0 ← Coefficient list for first segment
	BIT #DEPRPT,208.(R0)	;Is there a departure point
	BEQ MOVSPL		; No - go on to the splining
	ADD #16.,R0
	MOV R0,R1
	ADD #200.,R1		;R1 ← Coefficient list for second segment
	LDF 8.(R1),AC0		;AC0 ← Seg time (C3)
	MOV #6,R2		;6 joints to do
3$:	LDF 12.(R1),AC1		;AC1 ← joint difference: departure - initial = del
	NEGF AC1
	STF AC1,16.(R0)		;C5 = -del
	NEGF AC1
	ADDF AC1,AC1		;AC1 ← 2 * del
	STF AC1,12.(R0)		;C4 = 2 * del
	CLRF 8.(R0)		;C3 = 0.0
	CLRF 4.(R0)		;C2 = 0.0
	DIVF AC0,AC1		;AC1 ← 2 * del / seg time = velocity
	STF AC1,4.(R1)		;C2(next segment) = initial velocity = 2 * del
	ADD #24.,R0		;R0 ← C1 for next joint
	ADD #24.,R1		;R1 ← C1 for next joint
	SOB R2,3$		;Do all 6 joints

MOVSPL:	CLR R5			;We have 6 joints to fit splines to
	PUSH R4			;So we have another register to use
1$:	MOV 2(SP),R4
	ADD #8.,R4		;R4 ← Coefficient list
2$:	MOV R4,R0		;R0 ← first segment in chunk
	ADD #200.,R4		;R4 ← last segment in chunk
	BIT #DEPRPT,(R4)	;See if this is a departure point
	BNE 2$			; Yes - already done, go on to next chunk
	MOV #1,R2		;R2 ← # of segments in chunk
	MOV R5,R1		;R1 ← # of joint to do this time through
	MUL #24.,R1		;R1 ← Offset into coefficient list for this joint
	ADD #8.,R1
	ADD R0,R1
	PUSH R1			;Save it for CUBSPL
	ADD #200.,R1		;R1 ← 1st seg of chunk
3$:	BIT #DESTPT+VELOC,(R4)	;See if velocity specified for this point
	BNE 4$			; Yes - go spline this chunk
	LDF 12.(R1),AC0		; No - Check if velocity changes direction
	ADD #200.,R1		;	between this segment & the next
	MULF 12.(R1),AC0	;AC0 ← C4 * C'4 < 0 if velocity changes direction
	CFCC
	BMI 4$			;Velocity changes - use 0 velocity at this point
	ADD #200.,R4		;R4 ← Next segment in chunk
	INC R2			;Update # of segments in chunk
	BR 3$
4$:	MOV R2,R1		;R1 ← # segments in chunk
	POP R2			;R2 ← C1 for start of chunk
	JSR PC,CUBSPL		;Do a spline fit for this chunk
	BIT #APPRPT,(R4)	;See if this is an approach point
	BEQ 6$			; No
	MOV #4,R1		; Yes - copy approach polynomial into proper place
	ADD #4,R2		;Need to copy C2 - C5
5$:	LDF 200.(R2),AC0	;Get approach polynomial coefficient
	STF AC0,(R2)+		;Store it in "last" segment
	SOB R1,5$		;Copy all 6 coefficients
	BR 7$			; then go on to next joint
6$:	BIT #DESTPT,(R4)	;Finished with this joint?
	BEQ 2$			; No - do next chunk
7$:	INC R5			; Yes - go on to next joint
	CMP #6,R5		;Have we done all 6 joints
	BNE 1$			; No - do next joint

	MOV R4,R0		;R0 ← "extra segment" - put wobble pointer here
	ADD #2,R0		;Leave the control word, so we know when to stop
	POP R4			;Restore R4 - Trajectory all set up
	MOV (SP),R1		;R1 ← Coefficient list
	ADD #8.,R1
	MOV #200.,R5		;Segment length
8$:	MOV R5,(R1)		;Done with control bits, store rel seg length
	ADD R5,R1		;Move onto next segment
	BIT #DESTPT,(R1)	;See if more to do
	BEQ 8$			; Yup - finish with other segments
	CLR (R1)		;Zero here so arm code knows where to end

MOVEND:	POP <R0,R5>	;R0 ← Coefficient list, restore R5
	PUSH R0		;Save R0 for MOVSTA
	MOV LMOVE,R2	;Set for moving operation
	JMP MOVSTA	;Use the common move code

;	 MOVE (auxillary routine: CUBSPL)

CUBSPL:	;the polynomial spliner - from de Boor's "A Practical Guide to Splines"
	; Calculates a trajectory for given joint starting at R2.
	; The number of segments in the chunk is given by R1.
	; Returns with R2 pointing to the start of the next chunk.

; IF n = 1 THEN ! Special case if no intermediate points;
;   BEGIN
;   dtau ← c[3,2];
;   divdf3 ← c[2,1] + c[2,2] - 2*c[4,2];
;   c[3,1] ← (c[4,2] - c[2,1] - divdf3) * dtau;
;   c[4,1] ← divdf3 * dtau;
;   c[2,1] ← c[2,1] * dtau;
;   RETURN;	      ! Go on to next chunk;
;   END;

	CMP #1,R1		;Only one segment?
	BNE 1$			; No - skip ahead for multiple segments
	LDF 208.(R2),AC0	;AC0 ← c[3,2] = dtau
	LDF 4(R2),AC1		;AC1 ← c[2,1]
	STF AC1,AC3		;AC3 ← c[2,1], need a copy later
	ADDF 204.(R2),AC1	;AC1 ← c[2,1] + c[2,2]
	LDF 212.(R2),AC2	;AC2 ← c[4,2]
	SUBF AC2,AC1		;AC1 ← c[2,1] + c[2,2] - 2*c[4,2] = divdf3
	SUBF AC2,AC1
	SUBF AC3,AC2		;AC2 ← c[4,2] - c[2,1]
	SUBF AC1,AC2		;AC2 ← c[4,2] - c[2,1] - divdf3
	MULF AC0,AC2		;AC2 ← (c[4,2] - c[2,1] - divdf3) * dtau = c[3,1]
	STF AC2,8.(R2)		;Store away c[3,1]
	MULF AC0,AC1		;AC1 ← divdf3 * dtau = c[4,1]
	STF AC1,12.(R2)		;Store away c[4,1]
	MULF AC0,AC3		;AC3 ← c[2,1] * dtau = c[2,1]
	STF AC3,4(R2)		;Store away c[2,1]
	CLRF 16.(R2)		;C5 = 0
	CLRF 20.(R2)		;C6 = 0
	BR 5$			;All done

; c[3,1] ← 0;
; c[4,1] ← 1;	! Slope prescribed at left end;
; FOR i ← 2 TIL nseg DO ! Generate interior knot eqns & forward pass of Gauss elim;
;   BEGIN
;   g ← -c[3,i+1] / c[4,i-1];
;   c[2,i] ← g * c[2,i-1] + 3 * (c[3,i] * c[4,i+1] + c[3,i+1] * c[4,i]);
;   c[4,i] ← g * c[3,i-1] + 2 * (c[3,i] + c[3,i+1]);
;   END;

1$:	CLRF 8.(R2)		;c[3,1] ← 0
	LDF ONE,AC0		;c[4,1] ← 1	Slope prescribed at left end
	STF AC0,12.(R2)
	MOV R1,R0		;R0 ← nseg
	DEC R0			;R0 ← nseg - 1
2$:	ADD #200.,R2		;R2 ← c[1,i], move on to next segment: (2,nseg)
	LDF 208.(R2),AC0	;AC0 ← c[3,i+1]
	STF AC0,AC1		;AC1 ← c[3,i+1]
	STF AC0,AC3		;AC3 ← c[3,i+1]
	DIVF -188.(R2),AC0	;AC0 ← c[3,i+1] / c[4,i-1]
	NEGF AC0		;AC0 ← -(c[3,i+1] / c[4,i-1]) = g
	MULF 12.(R2),AC1	;AC1 ← c[3,i+1] * c[4,i]
	LDF 8.(R2),AC2		;AC2 ← c[3,i]
	ADDF AC2,AC3		;AC3 ← c[3,i] + c[3,i+1]
	MULF 212.(R2),AC2	;AC2 ← c[3,i] * c[4,i+1]
	ADDF AC1,AC2		;AC2 ← c[3,i] * c[4,i+1] + c[3,i+1] * c[4,i] = t
	MULF THREE,AC2		;AC2 ← 3 * t
	LDF -196.(R2),AC1	;AC1 ← c[2,i-1]
	MULF AC0,AC1		;AC1 ← g * c[2,i-1]
	ADDF AC2,AC1		;AC1 ← g * c[2,i-1] + 3 * t = c[2,i]
	STF AC1,4.(R2)
	LDF -192.(R2),AC1	;AC1 ← c[3,i-1]
	MULF AC0,AC1		;AC1 ← g * c[3,i-1]
	ADDF AC3,AC1		;AC1 ← g * c[3,i-1] + 2 * (c[3,i] + c[3,i+1])
	ADDF AC3,AC1		;AC1 = c[4,i]
	STF AC1,12.(R2)
	SOB R0,2$		;Do all the interior segments

; FOR i ← nseg STEP -1 UNTIL 1 DO	! Carry out back substitution;
;   c[2,i] ← (c[2,i] - c[3,i] * c[2,i+1]) / c[4,i];

	MOV R1,R0		;R0 ← nseg
3$:	LDF 8.(R2),AC0		;AC0 ← c[3,i]
	MULF 204.(R2),AC0	;AC0 ← c[3,i] * c[2,i+1]
	LDF 4(R2),AC1		;AC1 ← c[2,i]
	SUBF AC0,AC1		;AC1 ← c[2,i] - c[3,i] * c[2,i+1]
	DIVF 12.(R2),AC1	;AC1 ← (c[2,i] - c[3,i] * c[2,i+1]) / c[4,i]
	STF AC1,4(R2)		;AC1 = c[2,i], store it
	SUB #200.,R2		;R2 ← c[1,i-1], move on to next segment: (nseg,1)
	SOB R0,3$		;Do all the segments

; FOR i ← 1 TIL nseg DO	! Generate the cubic coefficents in each interval;
;   BEGIN		! & Stow away the answer into the coefficient array;
;   dtau ← c[3,i+1];
;   divdf1 ← (c[1,i+1] - c[1,i]) / dtau;
;   divdf3 ← c[2,i] + c[2,i+1] - 2*divdf1;
;   c[3,i] ← (divdf1 - c[2,i] - divdf3) * dtau;
;   c[4,i] ← divdf3 * dtau;
;   c[2,i] ← c[2,i] * dtau;
;   END;

4$:	ADD #200.,R2		;R2 ← c[1,i], move on to next segment: (1,nseg)
	LDF 208.(R2),AC0	;AC0 ← c[3,i+1] = dtau
	LDF 200.(R2),AC1	;AC1 ← c[1,i+1]
	SUBF (R2),AC1		;AC1 ← c[1,i+1] - c[1,i]
	DIVF AC0,AC1		;AC1 ← (c[1,i+1] - c[1,i]) / dtau = divdf1
	LDF 4(R2),AC2		;AC2 ← c[2,i]
	STF AC2,AC3		;AC3 ← c[2,i]
	ADDF 204.(R2),AC2	;AC2 ← c[2,i] + c[2,i+1]
	SUBF AC1,AC2		;AC2 ← c[2,i] + c[2,i+1] - 2*divdf1 = divdf3
	SUBF AC1,AC2
	SUBF AC3,AC1		;AC1 ← divdf1 - c[2,i]
	SUBF AC2,AC1		;AC1 ← divdf1 - c[2,i] - divdf3
	MULF AC0,AC1		;AC1 ← (divdf1 - c[2,i] - divdf3) * dtau = c[3,i]
	STF AC1,8.(R2)
	MULF AC0,AC2		;AC2 ← divdf3 * dtau = c[4,1]
	STF AC2,12.(R2)
	MULF AC0,AC3		;AC3 ← c[2,i] * dtau = c[2,1]
	STF AC3,4.(R2)
	CLRF 16.(R2)		;C5 = 0
	CLRF 20.(R2)		;C6 = 0
	SOB R1,4$		;Do all the segments

5$:	ADD #200.,R2		;R2 ← c[1,nseg+1], move on to next chunk
	RTS PC			;All done

;	 MOVE (auxillary routines: MOVARG, MOVPOS & move data)

MOVARG:	;Auxillary routine called by MOVE to get argument value on R3 stack
	;Scalar value is either on R3 stack, a constant, or a variable
	FETCH R0		;Get where argument is
	CMP #-1,R0		;Already on stack?
	BEQ 2$			; Yup - all done
	BIT #140000,R0		;Constant value?
	BEQ 1$
	MOV R0,-(R3)		;Push constant value onto stack
	BR 2$
1$:	PUSH R2
	JSR PC,GTVAL0		;Push value of variable onto stack
	POP R2
2$:	RTS PC

MOVPOS:	;Auxillary routine called by MOVE to take the trans on the R3 stack, do
	;an arm solution for that position using SOLVE, compute joint inertias
	;using DTERMS, and store all that info away in the trajectory.
	PUSH R2			;Save pointer to tables of pointers
	ADD #2,R2		;R2 ← pointers to last segs joint angles
	MOV #6,R0		;6 joints to copy & update pointers for
1$:	LDF @(R2),AC0		;Get last joint angle value
	ADD #200.,(R2)		;Update pointer to this seg
	STF AC0,@(R2)+		;Copy old joint angle
	ADD #200.,12.(R2)	;Two joint pointer lists to update
	SOB R0,1$
	MOV (R3)+,R0		;R0 ← New arm position
	MOV (SP),R1
	MOV (R1)+,R2		;R2 ← mech bits, R1 ← start of joint pointer table
	JSR PC,@LSOLVE		;Compute the arm solution
	TST R0			;Was it okay?
	BEQ 2$			; Yup
	MOV R0,-(SP)		; No - Save out of range joint(s)
	MOV #CRLFX,R0		;  Move to new line
	JSR PC,TYPSTR		;
	MOV #BADJTM,R0		;  Complain about no arm solution
	JSR PC,TYPSTR
	MOV (SP)+,R0		;  Restore &
	JSR PC,TYPOCT		;  Print out of range joint(s)
	ALWARN NOSOLM		;  And tell him we're proceeding anyway
2$:	MOV (SP),R0
	MOV (R0)+,R2		;R2 ← mech bits, R0 ← start of joint pointer table
	MOV R0,R1
	ADD #28.,R1		;R1 ← table of joint inertia pointers
	JSR PC,@LDTERMS		;Compute the gravity loading & inertia terms
	MOV #12.,R0		;12 inertia terms
3$:	ADD #200.,(R1)+		;Update the inertia pointers for next seg
	SOB R0,3$
	MOV (SP),R1		;R1 ← table of pointers to joint angles
	MOV #6,R0		;Going to compute change in position for each joint
	CLRF AC1		;Also want to find max time needed
	MOV #YJT1,R5		;R5 ← joint rates for Yellow arm
	BIT #YARM,(R1)+		;Check which arm
	BNE 4$			; Yellow - all set
	MOV #BJT1,R5		;R5 ← joint rates for Blue arm
4$:	MOV (R1)+,R2		;R2 ← address of new joint value
	LDF (R2),AC0		;Get new position
	SUBF -200.(R2),AC0	;Compute joint difference: New - Old
	STF AC0,12.(R2)		;Store it in C4
	ABSF AC0		;Make sure it's positive
	CMPF FIFTN,AC0		;For small joint changes use more time
	CMP #4,R0		;Treat joint 3 specially
	BNE 5$			; Other joints are in degrees
	CMPF THREE,AC0		; But it's in inches
5$:	CFCC
	BMI 6$			;Not a small motion - use faster rate
	ADDF AC0,AC0		;Make small motions take twice as long
6$:	MULF (R5)+,AC0		;Compute time needed = rate * difference
	CMPF AC1,AC0		;See if it needs more time than current max
	CFCC
	BPL 7$			;No, it's smaller
	STF AC0,AC1		;Yes - make it new max time
7$:	SOB R0,4$		;Do all 6 joints
	TST (R1)+
	MOV (R1),R1		;R1 ← ptr to 1st joint coefficients
	STF AC1,8.(R1)		;Store max time temporarily in C3
	POP R2			;Restore pointer to pointer tables
	RTS PC			;All done here

DATA
YJT1::	.FLT2 0.00916	; 0.55 / 60	; Yellow joint times in seconds
YJT2::	.FLT2 0.00533	; 0.32 / 60	;	jiffies/60
YJT3::	.FLT2 0.04500	; 2.7 / 60
YJT4::	.FLT2 0.00250	; 0.15 / 60
YJT5::	.FLT2 0.00250	; 0.15 / 60
YJT6::	.FLT2 0.00400	; 0.24 / 60
YJT7::	.FLT2 0.11666	; 7.0 / 60

BJT1::	.FLT2 0.00913	; 0.55 / 60	;Blue joint times
BJT2::	.FLT2 0.00750	; 0.45 / 60
BJT3::	.FLT2 0.04500	; 2.7 / 60
BJT4::	.FLT2 0.00250	; 0.15 / 60
BJT5::	.FLT2 0.00250	; 0.15 / 60
BJT6::	.FLT2 0.00400	; 0.24 / 60
BJT7::	.FLT2 0.11666	; 7.0 / 60

HANDIN:: .FLT2 1590000.0	;Hand inertia
				;If Yellow hand is different need to change
MAXHD::	.FLT2 3.90		;Should have value for each hand, but....
				;  should also have a MINHD....
TWOTH::	.FLT2 0.2
FIFTNH::.FLT2 0.15
ONEHF::	.FLT2 1.5
FIFTN:: .FLT2 15.0
THREE:: .FLT2 3.0

NOTFRM:: .ASCIZ /Can't use non-trans as a control frame in MOVE!!!/
NODEVM:: .ASCIZ /Control frame not affixed to any device: Assuming barm./
TIMERR:: .ASCIZ /Not enough time allocated for motion - nonetheless using it./
DURERR:: .ASCIZ /Ignoring overall duration request - motion overly constrained./
NOSOLH:: .ASCIZ /INVALID FINGER OPENING - WILL USE APPROPRIATE STOP LIMIT./
BADJTM::  ASCIE /NO VALID ARM SOLUTION FOUND - JOINT(S) OUT OF RANGE: /
NOSOLM::  ASCIE /WILL USE AN APPROXIMATE TRAJECTORY./

CODE
;	 CENTER, UPDEPR, OPERATE, STOP

CENTER:	;Interpreter routine
	MOV #10,R0		;Size of coefficient list needed by CENTER
	JSR PC,GTFREE		;R0 ← Address of coefficient list
	PUSH R0			;Save address for MOVSTA
	MOV #BARMSB+BHANDSB,(R0) ;Set up servo bits for Blue arm
	BIT #YARM,@(R4)		;Now see if we were right
	BEQ 1$			; Yup - it's the Blue arm so skip ahead
	MOV #YARMSB+YHANDSB,(R0) ;Set up servo bits for Yellow arm
1$:	FETCH CMECH(R4)		;Store away mech bits
	MOV LCENTER,R2		;Set for centering operation
	JMP MOVSTA		;Use the common move code

UPDEPR:	;Interpreter routine
	MOV CMECH(R4),R0	;R0 ← Mech bits of arm last moved
	CMP #YARM,R0
	BNE 1$
	MOV YAPPR+2,YDEPR+2	;Update departure point for next move
1$:	CMP #BARM,R0
	BNE 2$
	MOV BAPPR+2,BDEPR+2	;Update departure point for next move
2$:	CCC
	RTS PC

OPERATE:;Interpreter routine
	MOV LOPERATE,R2	;Set for device operation
	MOV #'π,R0	;Whistle while you work
	JSR PC,TYPCHR
	MOV #12,R0	;Get a block for the coefficient list
	JSR PC,GTFREE
	PUSH <R0>	;Save a copy on the stack
	FETCH (R0)+	;Store in servo bits
	CLR (R0)+	;Clear 2nd word of servo bits
	FETCH (R0)+	;Store in command bits
	CLR (R0)+	;Clear wobble ptr
	CLR (R0)+	;Clear rel seg ptr
	FETCH CMECH(R4)	;Store away mech bits
	LDF @(R3)+,AC0	;AC0 ← time allocated for operation
	ABSF AC0	;Make sure time is positive
	MULF THOUS,AC0	;AC0 ← time, in milliseconds
	STCFI AC0,(R0)+	;Store away time
	LDF @(R3)+,AC0	;AC0 ← vise stop-wait time or driver torque
	LDF @(R3)+,AC1	;AC1 ← vise position or driver velocity
	BIT #VISESB,@(SP) ;Check whether vise or driver
	BEQ 1$		;Handle driver below
	MULF THOUS,AC0	;Convert stop-wait time to milliseconds
	BR 2$
1$:	MULF THOUS,AC1	;Convert driver velocity to degrees/millisecond
2$:	STF AC0,(R0)+	;Store them away
	STF AC1,(R0)+
	MOV #DVBKSZ,R0	;Get a device block
	JSR PC,GTFREE
	MOV R0,R1	;R1 ← address of device block
	MOV (SP)+,R0	;R0 ← coefficient list
	JMP OPMOV	;Use the common move code

STOP:	;Interpreter routine
COMMENT ⊗ Takes one argument, the level-offset of the control frame to stop.
Determines the appropriate mechanism and all the associated joints are stopped. ⊗

	JSR PC,GETMEC		;R2 ← mech bits for specified control frame
STOP0:				; entry point for POINTY
	MOV R2,R0		;Copy them for TABOFS
	JSR PC,TABOFS		;R0 ← table offset
	BIT #ANARM,R2		;An arm?
	BEQ 1$			;No
	MOV #6,R1		;R1 ← count of joints
	BR 2$
1$:	MOV #1,R1		;R1 ← count of joints
2$:	ADD LDVCPTR,R0		;R0 ← LOC[table of device pointers]
3$:	MOV (R0)+,R2		;R2 ← device block
	BEQ 4$			;If any
	TST (R2)		;Make sure still valid
	BEQ 4$
	BIS #100000,@0(R2)	;Stop this device.
4$:	SOB R1,3$		;Repeat
	SLEEP #200.		;Sleep for 200 milliseconds so arm can stop
	CCC			;Clear condition code
	RTS PC			;Done

;Common code for motions: MOVSTA & OPMOV

DVBKSZ == 24		;Size of a device block (2+#jts)

MOVSTA:	MOV #'π,R0	;Whistle while you work
	JSR PC,TYPCHR
	MOV #DVBKSZ,R0	;Get a device block
	JSR PC,GTFREE
	MOV R0,R1	;R1 ← address of device block
	POP <R0>	;R0 ← address of coefficient list

OPMOV:	PUSH <R1,R0>	;Save pointers so we can reclaim these later

41$:	CMPB SAILID,#2	;See if we're talking to the 10
	BNE 42$		;Nope - skip ahead
	TST NOTB10	;Gathered force data still there?
	BEQ 42$		;Nope - skip ahead
	TST NOTB10+2	;Check data valid flag - if still set wait
	BEQ 42$		;Nope - skip ahead
	SLEEP #144	;No - wait 100 msecs then try again
	BR 41$

42$:	JSR PC,NOCMP	;Don't compact for a bit
.IFNZ	CPOINTY
	MOV #1,DSPOK	; kill the display for a while
.ENDC
	JSR PC,@R2	;Do some kind of move (MOVE, CENTER, OPERATE)
.IFNZ	CPOINTY
	CLR DSPOK	; turn on the display again
.ENDC
	LDCIF R0,AC0	;Convert error bits to scalar
	PUSH <R0>	;Save them too
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar]
	STF AC0,(R0)

	CMPB SAILID,#2	;See if we're talking to the 10
	BNE 40$		;Nope - skip ahead
	BIT #ANARM,CMECH(R4)	;See if we just moved an arm
	BEQ 40$		;Nope - skip ahead
	TST NOTB10	;Any gathered force data to send over?
	BEQ 40$		;Nope - skip ahead
	TST NOTB10+2	;Check data valid flag - shouldn't be set yet
	BNE 40$		;Already have signalled 10
	INC NOTB10+2	;Set data valid
;	MOV #1,172566	;Wake up pdp10 by generating interrupt
	CMP FPTR,#PCODE	;Check if force data has overwritten pcode
	BLO 40$		;Okay - skip ahead
	ALERR GATERR	;Complain

	;Update error variables - also any other device specific cleanup
40$:	MOV CMECH(R4),R2	;R2 ← mechanism bits
	BIT #YARM,R2
	BEQ 1$
	MOV (R3),YAERR+2
1$:	BIT #YHAND,R2
	BEQ 2$
	MOV (R3),YHERR+2
2$:	BIT #BARM,R2
	BEQ 3$
	MOV (R3),BAERR+2
3$:	BIT #BHAND,R2
	BEQ 4$
	MOV (R3),BHERR+2
4$:	BIT #DRIVER,R2
	BEQ 5$
	MOV (R3),DRERR+2
5$:	BIT #VISE,R2
	BEQ 7$
	CALL GETVAL,<#FJAWH>	;Try to get a value for the fixed jaw
	TST R0			;Did we suceed?
	BEQ 6$			;Nope
	MOV #MJAWH,R0		;Header for MOVING-JAW
	JSR PC,INVLDT
	CLR FJAWH+INVMRK	;Mark FIXED-JAW as still valid
6$:	MOV #VISEH,R2
	JSR PC,GETDEV		;Get current vise opening
	LDF @(R3)+,AC0		;Put it in AC0
	MOV VISOP+2,R0		;R0 ← LOC[vise opening trans]
	STF AC0,50(R0)		;Update y value in trans
	MOV (R3),VIERR+2
7$:	TST (R3)+		;Flush device error bits
	JSR PC,YESCMP		;OK to compact now

	POP <R2,R0>		;R2 ← error bits, R0 ← coefficient list
	JSR PC,RLFREE		;Release coefficient list
	BIT #135400,R2		;Associated joint #?
	BEQ 11$			; No - skip ahead
	MOV (SP),R1		;Get address of device block
	MOV (R1)+,R0		;Maximum number of joints in device block
	TST (R1)+		;Point to first joint
9$:	BIT #177400,(R1)+	;Is this the offending joint?
	BNE 10$			; Yup - found it
	SOB R0,9$		;Try next joint
	BR 11$
10$:	MOV -(R1),R2		;Change R2 so the low 2 digits give joint #
11$:	POP <R0>
	JSR PC,RLFREE		;Get rid of the device block
	FETCH R0		;Get error bit mask
	TST R2			;Any errors?
	BEQ 30$			;Nope - all done
	BIT #177400,R2		;Any high byte bits on?
	BEQ 20$			;No, handle low byte errors ourselves
	MOV R2,R1
	BIC #177,R1		;R1 ← error bits - jt #
	BIC R0,R1		;Is this what the poor user wants to handle himself?
	BNE 20$			;No - handle it below
	BMPIPC			;Bump IPC past address of next pcode
	BMPIPC			;Bump IPC past retry address
	CCC
	RTS PC			;Go do the user's error handling code

20$:	MOV R2,R0
	PUSH <R0>		;  save error code
	EVWAIT CSLEVT		;Grab the console
	CMP R0,#7		;Power supply off?
	BNE 21$
	MOV MOVERS,R0		;Yup - R0 ← address of error message
	BR 22$
21$:	CMP R0,#16		;Background force job over run?
	BNE 23$
	MOV MOVERS+2,R0		;Yup - R0 ← address of error message
22$:	JSR PC,TYPSTR		;  Complain
	POP <R0>
	BR 26$
23$:	MOV #2000,R1	;Check for: Panic button pushed, Excessive force, Time out,
			; Stop limit exceeded & No arm solution while force servoing
	MOV #MOVERS+4,R2	;Pointer to list of error message addresses
24$:	BIT R0,R1		;Is this the error?
	BNE 25$			;Yes - found it
	TST (R2)+		;Advance error message pointer
	ASL R1			;Try next servo error
	BPL 24$		;Go see if this is the one - unless we've checked them all
25$:	MOV (R2),R0		;R0 ← address of error message
	JSR PC,TYPSTR		;  Complain
	POP <R0>
	BIC #76000,R0	;(exclude ex.force,timeout,joint lim,panic but & no arm sol)
	BEQ 26$
	JSR PC,TYPOCT		;  Give error condition
26$:	EVSIG CSLEVT		;Release the console
	ALERR MOVERR		;  and switch to DDT
30$:
.IFNZ	CPOINTY
	CMPB SAILID,#1		; is it POINTY?
	BNE  31$		; no, take the AL return
	JMP  RJMP		; yes, do a relative jump
.ENDC
31$:	JMP JUMP		;Jump to next pcode address

DATA
MOVERS:	.WORD 1$,2$,3$,4$,5$,6$,7$,8$		;Pointers to error messages

1$:	.ASCIZ /
ARM INTERFACE POWER SUPPLY TURNED OFF
(CHECK JOINT BRAKE SWITCHES)/			; 7
2$:	.ASCIZ /
BACKGROUND JOB TOOK TO LONG TO EXECUTE /	;16
3$:	.ASCIZ /
PANIC BUTTON PUSHED/				; 2000
4$:	.ASCIZ /
EXCESSIVE FORCE ENCOUNTERED BY JOINT /		; 4000
5$:	.ASCIZ /
TIME OUT FOR JOINT /				; 10000
6$:	.ASCIZ /
STOP LIMIT EXCEEDED FOR JOINT /			; 20000
7$:	.ASCIZ /
NO ARM SOLUTION WHILE DOING FORCE COMPLIANCE /	; 40000
8$:	.ASCIZ /
SERVO ERROR = /

MOVERR::ASCIE </to retry the move, RETRY$G
to move arm directly to destination, FINISH$G/>
GATERR::ASCIE </PCODE overwritten by gathered force data!/>

CODE
;Error recovery for motions: RETRY, FINISH, PARK

RETRY:	TST (SP)+	;Get here from ALERR; clean off stack
.IFNZ CPOINTY
;	MOV #1,RPFLAG	;Tell POINTY that this is a RETRY
.ENDC
RETRY1:	BMPIPC		;Bump IPC - to retry address
.IFNZ CPOINTY
	CMPB SAILID,#1
	BNE 1$		; no it is not Pointy
	JMP RJMP	; do a relative jump
.ENDC
1$:	JMP JUMP	;Try the whole move again - from the top 

FINISH:	TST (SP)+		;Get here from ALERR; clean off stack
	BIT CMECH(R4),#ANARM	;Make sure it's an arm
	BEQ RETRY1		;Otherwise just do a RETRY
	PUSH (R4)		;Save IPC
1$:	MOV #FIVE,-(R3)		;Take 5 seconds for the motion
	BIT CMECH(R4),#BARM	; Unless we're not
	BNE 2$
	MOV YFINI,R0		;Moving yarm
	CLR YDEPR+2		;Don't use a deproach
	BR 3$
2$:	MOV BFINI,R0		;Moving barm
	CLR BDEPR+2		;Don't use a deproach
3$:	MOV R0,-(R3)		;Push destination position on stack
	MOV #FINMOV+2,(R4)	;IPC of dummy move
	JSR PC,FAKMOV		;Do it - indirect so RETRY will work
	CMP #FINMOV,(R4)	;See if attempting a RETRY
	BEQ 1$			;Yup - so try it all again
	POP (R4)		;Restore old IPC
	JMP JUMP		;Proceed with the program

FAKMOV::JSR PC,MOVE		;If the move succeeds we'll return here
	RTS PC			; otherwise RETRY will return us

PARK:	MOV #FIVE,-(R3)		;Take 5 seconds for the motion
	MOV #BPARK,-(R3)	;Moving to bpark
	MOV #NILVEC,-(R3)	;Zero velocity at VIA
	MOV #BAOFST,R0
	JSR PC,GTVAL0		;Get current blue arm position
	MOV (R3),R2
	LDF BPARK+44.,AC0	;Copy bpark height
	STF AC0,44.(R2)		;So we use a VIA point at the right height
	MOV #BPRKMV+2,(R4)	;IPC of dummy move
	JSR PC,FAKMOV		;Do it - indirect so RETRY will work
	CMP #BPRKMV,(R4)	;See if attempting a RETRY
	BEQ PARK		;Yup - so try it all again
		;Eventually we'll want to repeat this for the yellow arm
1$:	BPT			;Don't let user proceed beyond here
	ALWARN PNTMES		;Print the "Can't continue" message
	BR 1$			; & don't

DATA
	3
BPARK:	.FLT2	-1.0000000
	.FLT2	 .0000000
	.FLT2	 .0000000
	.FLT2	 .0000000
	.FLT2	 1.0000000
	.FLT2	 .0000000
	.FLT2	 .0000000
	.FLT2	 .0000000
	.FLT2	-1.0000000
	.FLT2	 43.5312500
	.FLT2	 56.8550000
	.FLT2	 9.9587500

FIVE::	.FLT2	5.0

BFINI::	0		;trans for FINISH to use for barm
YFINI::	0		;trans for FINISH to use for yarm

FINMOV:: XMOVE		;Pcode for finishing motions
	BAOFST		;which arm to move
	1		;one segment
	DESTPT		;Destination follows
	-1		;Value on stack
	DURLB+NULLING	;Use at least 5 seconds
	-1		;Value on stack
	0		;No error handler
	0		;No next move
	FINMOV		;For RETRY

BPRKMV:: XMOVE		;Pcode for parking blue arm
	BAOFST		;which arm to move
	3		;Two segments + maybe a departure
	VIAPT+VELOC	;Via point follows
	-1		;Position value on stack
	-1		;Velocity on stack
	DESTPT		;Destination follows
	-1		;Value on stack
	DURLB+NULLING	;Use at least 5 seconds
	-1		;Value on stack
	0		;No error handler
	0		;No next move
	BPRKMV		;For RETRY

CODE

;Force system routines: SETBAS, WRIST, STIFF, GATHR

	;Interpreter routine
SETBAS: CLR R0		;Don't return the matrix
	JSR PC,@LSETBAS	;Go calibrate the wrist
	CCC
	RTS PC		;All done

WRIST:	MOV #6*2,R0	;Get enough room to store 6 floating point force values
	;Interpreter routine
	JSR PC,GTFREE
	MOV R0,R1	;R1 ← address of device block
	PUSH <R0>	;Save a copy on the stack
	CLR R0		;Use internal calibration matrix
	JSR PC,@LWRIST	;Go read the wrist
	FETCH R0	;R0 ← offset for variable to store force vector in
	JSR PC,GETARG	;R0 ← LOC[env entry for force vector:K]
	PUSH <R0>	;Save it
	FETCH R0	;R0 ← offset for variable to store torque vector in
	JSR PC,GETARG	;R0 ← LOC[env entry for torque vector:G]
	PUSH <R0>	;Save this one too
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector]
	JSR PC,GETVEC	;R0 ← -(R3) ← LOC[new vector] - Get two of them
	POP <R0,R1>	;R0 ← G, R1 ← K
	MOV (R3),2(R1)	;Store pointer to force vector away in environment
	MOV 2(R3),2(R0)	; ditto for torque vector
	MOV (SP),R2	;R2 ← LOC[force components]
	MOV #2,R0	;# of vectors to transfer
1$:	MOV (R3)+,R1	;R1 ← LOC[force/torque vector]
	LDF (R2)+,AC0	;Get 1st force component
	STF AC0,(R1)+	;Store it in vector
	LDF (R2)+,AC0	; ditto for 2nd component
	STF AC0,(R1)+
	LDF (R2)+,AC0	; & likewise for 3rd component
	STF AC0,(R1)+
	SOB R0,1$	;Do both vectors
	POP <R0>	;R0 ← LOC[force component block]
	JSR PC,RLFREE	;Release it
	CCC
	RTS PC		;All done

STIFF:	MOV #14,R0	;Get a block to store the 6 stiffness values into
	;Interpreter routine
	JSR PC,GTFREE	;R0 ← address of block
	PUSH <R0>	;Save a copy on the stack
	MOV #2,R2
1$:	MOV (R3)+,R1	;R1 ← LOC[force/torque vector]
	LDF (R1)+,AC0	;Pick up stiffness value
	STF AC0,(R0)+	;Stuff it into block
	LDF (R1)+,AC0	;do it again
	STF AC0,(R0)+
	LDF (R1)+,AC0	;once more
	STF AC0,(R0)+
	SOB R2,1$
	JSR PC,GETARM	;See which arm we're using → R0
	FETCH R1	;Get bits for SETC: coord system (hand/table)
	BIS R1,R0	;R0 ← coord sys + arm (ignored now)
	MOV (SP),R0	;R0 ← LOC[stiffness value block]
	MOV (R3)+,R1	;R3 ← LOC[compliance center]
	JSR PC,@LSETSTF	;Call set stiffness routine
	POP <R0>
	JSR PC,RLFREE	;Release stiffness value block
	RTS PC		;All done

GATHR:	;Interpreter routine
	FETCH R0	;Get control bits
1$:	CMPB SAILID,#2	;See if someone on the 10 is there to talk with us
	BNE 3$
	TST NOTB10	;Data from last GATHER read yet?
	BEQ 2$		;Yup - skip ahead
	SLEEP #144	;No - wait 100 msecs then try again
	BR 1$
2$:	MOV #IPTR,R2	;Pointer for ID #, force bits & # pts - used by 10 program
	MOV #DATEND,(R2)
	MOV #FPTR,R1	;Pointer to where to store force readings
	MOV #DATEND+6,(R1)
	MOV #DATEND,NOTB10	;Set up pointer to data buffer for 10
	CLR NOTB10+2	;Clear data valid flag
	PUSH R3		;Save R3 stack
	INC GCNT	;Update GATHER counter
	MOV GCNT,R3
	JSR PC,@LGATHER	;Go tell force system we want force data gathered
	POP R3		;Restore R3 stack
3$:	CCC
	RTS PC		;All done

DATA
GCNT:	0		;Use a unique number for each gather
IPTR:	DATEND		;Pointer for ID #, force bits, & # pts
FPTR:	DATEND+6	;Pointer for force readings
CODE

;Motion auxilary functions: TABOFS, WHERE, NOTICE, GETARM, GETMEC

COMMENT ⊗ Certain tables are available via COMTAB entries.  LERRPTR
points to the table ERRPTR of 16 words, one for each servo, which
points at the current error torques.  LTHPTR points at the table THPTR
of 16 words, one for each servo, which points at the current joint
angles.  ⊗

TABOFS:	
COMMENT ⊗ R0 = Mechanism bit.  Returns table offset (in bytes) in R0.
For example, if the mechanism is BARM, the OFBARM is returned.  ⊗
	MOV #OFTAB,R1	;R1 ← start of mech/offset table
1$:	BIT (R1)+,R0	;Is it this mechanism?
	BEQ 2$		;No - skip ahead
	MOV (R1),R0	;Yes - Load up proper offset from table
	RTS PC		; and return.
2$:	TST (R1)+	;Advance to next table entry
	TST (R1)	;Check if at end of table
	BNE 1$		; & if more check them
	ALERR TABMES	;Illegal
	CLR R0
	RTS PC
DATA
OFTAB:	.WORD YARM, OFYARM, YHAND, OFYHAND, BARM, OFBARM, BHAND, OFBHAND
	.WORD VISE, OFVISE, DRIVER, OFDRIVER, 0
TABMES::ASCIE </ILLEGAL MECHANISM/>
CODE

WHERE:	;Interpreter routine
COMMENT ⊗ One argument: The mechanism bits.  Puts value of that
mechanism on the stack.  Only one mechanism at a time, please!  ⊗
	FETCH R2	;Mechanism bits
	JSR PC,NOCMP	;Don't compact for a bit
	BIT #ANARM,R2	;An arm?
	BEQ 1$		;No - skip
	JSR PC,GETTRN	;R0 ← -(R3) ← LOC[new trans]
	BR 2$
1$:	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar]
2$:	MOV LTHPTR,R1	;
	JSR PC,@LUPDATE	;
	JSR PC,YESCMP	;OK to compact now
	CCC		;Clear condition code
	RTS PC		;Done

NOTICE:
COMMENT ⊗ The arms may have been moved without our knowledge.  A call
to this routine invalidates all manipulator variables.  This routine 
should be called from WAITE, but not from MOVE or CENTER.  It may be 
called from DDT, since it saves all registers.  ⊗
	PUSH <R0,R1,R2>

COMMENT ⊗ Leave this out until Ken brings the vise up.
	CALL GETVAL,<#FJAWH>	;Try to get a value for the fixed jaw
	TST R0			;Did we suceed?
	BEQ 1$			;Nope
	MOV #MJAWH,R0		;Header for MOVING-JAW
	JSR PC,INVLDT
	CLR FJAWH+INVMRK	;Mark FIXED-JAW as still valid
1$:	MOV #VISEH,R2
	JSR PC,GETDEV		;Get current vise opening
	LDF @(R3)+,AC0		;Put it in AC0
	MOV VISOP+2,R0		;R0 ← LOC[vise opening trans]
	STF AC0,50(R0)		;Update y value in trans
⊗
	POP <R2,R1,R0>
	RTS PC

GETMEC:
COMMENT ⊗ One argument: the control frame.  Determines mech bits from it,
placing them in R0. If no device is associated with the frame barm is used. ⊗
	FETCH R0		;Get level-offset of control frame
GTMEC0:				; entry point for POINTY
	JSR PC,GETARG		;R0 ← Environment entry for cf
	BIT #HDRTYP,(R0)+	;Should have a header
	BEQ 2$			; It doesn't - just assume barm
	MOV (R0),R0		;R0 ← header
	BIT #FTYPE,TYPE(R0)	;See if it's a frame or a device
	BNE 1$			; Handle frames below
	MOV MECH(R0),R2		;R2 ← device mech bits
	BR 3$			;Now go stop it
1$:	BIT #DYNAM,TYPE(R0)	;Better be a dynamic frame
	BEQ 2$			; Nope - skip ahead & assume barm
	MOVB DCNT+1(R0),R2	;R2 ← mech bits
	BR 3$			;Now go stop it
2$:	MOV #BARM,R2		;Who knows what user wanted - just assume barm
3$:	RTS PC			;All done return


GETARM:	;Gets arm bit needed in calls to the force system & puts it in R0
	JSR PC,GETMEC		;R2 ← Mech bits
GTARM0:				; entry point for POINTY
	BIT #YARM,R2		;Yellow arm?
	BEQ 1$			; No
	MOV #1,R0		; Yes
	BR 2$
1$:	MOV #4,R0		;Blue arm if not yellow
2$:	RTS PC			;All done


;Condition monitors:  CMMAK 

COMMENT ⊗ This is the third version of condition monitors: modified by arg 5/77
(here refered to as c-m's).  Hardware-type c-m's will be ready soon. (hah! 1/78)
The basic operations are Creation, Enabling, Disabling, Destruction.
Creation causes a c-m control block to be set up, and pointed to by
the c-m variable.  This block has the following fields: ⊗

	II == 0
	XX	CMTYPE	;Type of c-m: event,expression,duration,force or hardware
	    CMEVT == 0		;Event type c-m
	    CMEXP == 1		;Expression type c-m
	    CMDRA == 2		;Duration type c-m
	    CMFRC == 3		;Force sensing type c-m
	    CMHRD == 4		;Hardware monitor type c-m
	XX	CMISB	;LOC[ISB] of the c-m
	XX	CMSTRT	;Starting address of c-m
	XX	CMBITS	;Bits needed for: force & hardware c-m's
	XX	CMSTAT	;Status bits for the c-m
            CMENB == 1               ;set => enabled
            CMDES == 2               ;set => to be destroyed
            CMRUN == 4               ;set => c-m is currently running
	CMCBSZ == II/2	;Length in words of a c-m control block.
	II == 6		;for event & expression c-m's
	XX	CMSEVT	;The event used to awaken the tester upon enabling

COMMENT ⊗ The various types of condition monitors are each handled
differently. Basically each c-m is an independent process which runs
in parallel with the process that creates it. Each c-m is an interpreter
and runs at priority 2 (exception: the checker part of an expression c-m
runs at priority 3). When a c-m is created by CMMAK, new PDB, ISB and
CMCB blocks are made. For duration, force and hardware c-m's nothing
further is done until they are enabled or destroyed. Enabling causes
the c-m checker part to be interpreted and to place the c-m body in the
appropriate queue, so it will be run if & when the condition being
checked for occurs. Disabling removes the c-m from the queue. Destroying
the c-m causes it to be disabled and then it's PDB, ISB & CMCB are all
reclaimed. At the conclusion of the body if the c-m has been re-enabled
it reschedules itself in the appropriate queue and then dismisses.
     Event and expression c-m's, after initialization, wait for the
gronking event CMSEVT.  Enabling signals the event CMSEVT and sets
the enabled bit in CMSTAT.  Disabling resets the enabled bit, and the
c-m will wait on the CMSEVT for future action.  As long as the c-m is
enabled, it periodically wakes up, checks its status bits.  If the
enable bit is reset, the c-m waits for CMSEVT.  Else it checks the
condition.  If it is satisfied, the c-m disables itself and
proceeds to the conclusion (at level 2, the conclusion should reset
itself to level 1 after all critical activity has been accomplished,
although this is not currently done.) Otherwise, it reschedules itself.
If the destroy bit should ever be set in CMSTAT, then the c-m will
destroy the event CMSEVT.  Then it will reclaim the c-m control blocked
and will dismiss, never to return. (The pointer to the c-m in the
environment should be zeroed by the destroying angel.). ⊗

CMMAK:	;Auxillary routine

COMMENT ⊗ Takes three (or four) arguments: the type of the nascent c-m, the
IPC of the c-m code, the environment size needed, & optionally the level-offset
of the event that this monitor is to wait on, or the bits needed to specify
force sensing. Called with the number of cmons to make in R0 and R2 pointing
at the environment entry. ⊗

1$:	PUSH <R0>		;Save count of how many cmons to make

	;Make a c-m control block
	MOV #CMCBSZ,R0
	JSR PC,GTFREE		;R0 ← LOC[c-m control block]
	MOV #CMNTYP,(R2)+	;Set data type to cmon
	MOV R0,(R2)+		;Stuff into environment
	PUSH <R2>		;Save environment pointer
	FETCH CMTYPE(R0)	;Get type of c-m
	PUSH <R0>		;Save LOC[c-m control block]

	;Prepare the c-m job
	FETCH CMSTRT(R0)	;Store away IPC of start of c-m code
	MOV CMSTRT(R0),R0	;R0 ← IPC of c-m code
	CLR R1			;C-m's do not expire with events
	JSR PC,SPAWN		;R0 ← process control block for c-m
	MOV (SP),R1		;R1 ← LOC[CMCB]
        MOV PDBR4(R0),R2	;R2 ← PR4 = LOC[c-m's interpeter status block]
	MOV R2,CMISB(R1)	;Store away location of c-m's ISB
        MOV R1,CMCB(R2)		;Stuff CMCB of the c-m
	MOV #UGRSAV+UFPUSE+4,PDBSTA(R0)	;c-m's run with priority = 2
	MOV #144040,UPSW(R0)
	CMP #CMEXP,CMTYPE(R1)	;If expression c-m runs with priority = 3
	BNE 2$
	MOV #UGRSAV+UFPUSE+6,PDBSTA(R0)	;Change priority to 3
	MOV #144140,UPSW(R0)
2$:	MOV R0,R2		;R2 ← new process descriptor block 

	;Set up the new environment
	JSR PC,NEWENV	;R0 ← LOC[new environment]
	MOV ENV(R4),SLINK(R0)	;Not necessary to set up OLEV, etc.
	MOV PDBR4(R2),R1
	MOV R0,ENV(R1)
	INC LEV(R1)

	POP <R0>		;R0 ← LOC[CMCB]
	CMP #CMEXP,CMTYPE(R0)	;See what type of c-m we've got
	BLT 4$			;Duration, force sensing or hardware - jump ahead
	EVMAK			;Expression or Event cmons
	POP <CMSEVT(R0)>	;Make an event for CMSEVT
	FORK R2,#INTERP,#USRDM	;Cause the c-m to be started.  It will go into wait.
	BR 5$			;Done

4$:	CMP #CMDRA,CMTYPE(R0)
	BEQ 5$			;If duration type then done
	FETCH CMBITS(R0)	;Get force sensing bits for c-m

5$:	POP <R2,R0>		;Retrieve env pointer & count
	DEC R0			;  & make as many cmons as we were told to
	BLE 6$
	JMP 1$
6$:	RTS PC			;Done

;  CMENBL, CMDSBL, CMDEST, CMTRIG, CMSKED, CMWAIT, CMUNCR

CMENBL: ;Interpeter routine
;  One argument, a level-offset pair for the c-m to enable.
	FETCH R0	;R0 ← level-offset
	JSR PC,GETARG	;R0 ← pointer into environment
	MOV 2(R0),R0	;R0 ← pointer to c-m control block.
	BEQ CMDERR	;If none, then error
	CMP #CMDRA,CMTYPE(R0)	;see what type of c-m we've got
	BGT 2$		;If event or expression then skip ahead
	BIT #CMENB,CMSTAT(R0)	;Already enabled?
	BNE 3$			;Then done
	BIS #CMENB,CMSTAT(R0)	;Set enabled bit
	BIT #CMRUN,CMSTAT(R0)	;See if currently running
	BNE 3$			; & if so we're done - it'll re-enable itself
	PUSH <R4>		;Save old ISB
	MOV CMISB(R0),R4	;Get new ISB
	MOV CMSTRT(R0),IPC(R4)	;Set IPC to LOC[c-m checker]
	MOV RF,-(SP)		;Save RF
	MOV SP,RF		;RF ← LOC[Stack]
	PUSH <#1$>		;Save return address since we're not doing a JSR PC
	PUSH <12(SP),12(SP)>	;Copy R3 stack limits
	JMP INT1		;Go do it - CMDUR, CMFORCE & CMSENSE return
1$:	POP <R4>		;Restore old ISB
	BR 2$		;Done
2$:	BIS #CMENB,CMSTAT(R0)	;Set the enable bit
	EVSIG CMSEVT(R0)	;Gronk the c-m
3$:	CCC		;Clear condition code
	RTS PC		;Done

CMDSBL:	;Interpreter routine
;  One argument, a level-offset pair for the c-m to disable.
	FETCH R0	;R0 ← level-offset
	JSR PC,GETARG	;R0 ← pointer into environment
	MOV 2(R0),R0	;R0 ← pointer to c-m control block.
	BEQ CMDERR	;If none, then error
	JSR PC,CMDIS	;Go disable the c-m
	CCC		;Clear condition code
	RTS PC		;Done
CMDERR:	ALERR CMNEMS
	SCC		;Set condition code
	RTS PC
DATA
CMNEMS::ASCIE </TRYING TO TREAT NON-EXISTENT EVENT/>
CODE

CMDEST:	;Auxillary routine
COMMENT ⊗ Called by KVAR to kill the cmon pointed at by (R2). ⊗
	PUSH <R1,R2>		;Save R1 & R2
	MOV (R2),R0		;R0 ← LOC[c-m control block]
	BEQ CMDERR		;If none, then error
	JSR PC,CMDIS		;Make sure c-m's disabled
	BIS #CMDES,CMSTAT(R0)	;Set the destroy bit
	CMP #CMDRA,CMTYPE(R0)	;See what type of c-m
	BGT 2$			;If event or expression c-m then handle below
	BIT #CMRUN,CMSTAT(R0)	;If running it will destroy itself
	BNE 3$			; so we're done	*** should probably wait though ***
	MOV CMISB(R0),R2	;R2 ← LOC[c-m's ISB]
	JSR PC,RLFREE		;Reclaim the c-m control block
	MOV STKBAS(R2),R0	;Reclaim interpreter stack
	JSR PC,RLFREE
	MOV ENV(R2),R0		;Reclaim this environment
	JSR PC,RLFREE
	MOV PDB(R2),R0		;Reclaim Process Descriptor Block
	JSR PC,RLFREE
	EVWAIT INTEVT		;Enter critical region.
	MOV #ISTBLK,R0	;The following unlinks this interpreter from the chain.
1$:	MOV R0,R1
	MOV NXTINT(R1),R0
	CMP R0,R2		;Have we found ours yet?
	BNE 1$
	MOV NXTINT(R2),NXTINT(R1)	; Yes. rechain.
	EVSIG INTEVT		;Leave critical region.
	MOV R2,R0		;Reclaim Interpreter Status Block
	JSR PC,RLFREE
	BR 3$
2$:	EVKIL CMSEVT(R0)	;Destroy the event.  That ought to wake him up!
	SLEEP #144		;Wait for it to die
3$:	POP <R2,R1>
	RTS PC			;Done

CMDIS:
COMMENT ⊗ Routine to disable a c-m, R0 ← LOC[CMCB] ⊗
	BIT #CMRUN,CMSTAT(R0)	;See if it is currrently running
	BEQ 10$			;  if not then proceed
	SLEEP #144		;  else wait for it to finish
	BR CMDIS		;  Checking every 100 milliseconds
10$:	BIT #CMENB,CMSTAT(R0)	;Check if currently enabled
	BEQ 3$			; if not - done
	CMP #CMDRA,CMTYPE(R0)	;See what type of c-m
	BGT 2$			;Event & expression c-m's are easy - skip ahead
	BEQ 2$			;Can't do anything with duration c-m's now
	CMP #CMFRC,CMTYPE(R0)
	BLT 2$			; ditto with hardware c-m's
	PUSH <R0>		;Save R0
	MOV CMISB(R0),R1	;R1 ← LOC[c-m's ISB]
	MOV PDB(R1),R1		;R1 ← LOC[c-m's PDB]
	MOV CMBITS(R0),R0	;R0 ← c-m's force sensing bits
	JSR PC,@LFRCOFF		;Remove c-m from force signal list
	TST R0
	BEQ 1$
;	ALERR CMNODS		;Complain if error (don't bother)
1$:	POP <R0>		;Restore R0
2$:	BIC #CMENB,CMSTAT(R0)	;Clear the enable bit
3$:	RTS PC			;Done

DATA
CMNODS::ASCIE </COULDN'T DISABLE FORCE CMON/>
CODE

CMTRIG:	;Interpeter routine
COMMENT ⊗ Should be executed only from a c-m.  Sets the priority to 1
and disables the checker.  ⊗

	MOV CMCB(R4),R0
1$:	EVTST CMSEVT(R0)	;Eat all signals enabling the checker.
	BCC 1$
	BIC #CMENB,CMSTAT(R0)	;Clear the enable bit
	BIS #CMRUN,CMSTAT(R0)	;Set the run bit
	SETPRI #1		;Set the priority to 1
	TST (SP)+		;Discard old priority
	CCC			;Clear condition code
	RTS PC			;Done

CMSKED:	;Interpreter routine
COMMENT ⊗ Goes to sleep a while (currently, 100 milliseconds).  Upon
awakening, checks the status bits of this checker, and either
dismisses, waits, or returns.  ⊗

	MOV CMCB(R4),R0	;R0 ← c-m control block
	BIC #CMRUN,CMSTAT(R0)	;Clear run bit
	CMP #CMEXP,CMTYPE(R0)	;See what type of c-m
	BNE 1$		;If event c-m skip ahead
	SETPRI #3	;In case the conclusion left it at 1
	TST (SP)+	;Flush old priority
	FETCH -(SP)	;Waiting interval
	SLEEP 		;Sleep a while
1$:	BIT #CMDES,CMSTAT(R0)	;Destroy bit set?
	BEQ 3$		;No
	EVKIL CMSEVT(R0);Yes.  Kill the triggering event.
2$:	JSR PC,RLFREE	;Return the c-m control block
	JMP TERMINATE	;Use the interpeter terminate routine.
3$:	BIT #CMENB,CMSTAT(R0)	;Enable bit set?
	BNE 4$		;Yes.
	EVWAIT CMSEVT(R0);No.  Wait until signaled by the enabler
	BCS 2$		;If the enabling event died, so must we.
	BR  1$		;Else start from the awakening point.
4$:	CCC		;Clear condition code
	RTS PC		;Done

CMWAIT:	;Interpreter routine.  
COMMENT ⊗  Used by event cmons to do the waiting.  ⊗

	FETCH R0	;R0 ← level-offset pair.
CMWAI0:			;entry point for POINTY
	JSR PC,GETARG	;R0 ← equivalent pointer into environment
	MOV 2(R0),R1	;R1 ← LOC[Event to wait for]
	EVWAIT R1	;Wait on that event.
	MOV CMCB(R4),R0	;R0 ← LOC[CMCB]
	BCC 1$		;Return okay? If the signaling event died, so must we.
	JSR PC,RLFREE	;Return the c-m control block
	JMP TERMINATE	;Use the interpeter terminate routine.
1$:	BIT #CMENB,CMSTAT(R0)	;Still enabled?
	BNE 2$		;Yes.  May exit.
	EVSIG R1	;Oops, we were disabled!  Resignal the event.
	MOV CMSTRT(R0),(R4)	;And try again. Reset IPC to start of cmon.
2$:	CCC		;Clear condition code
	RTS PC		;Done

CMUNCR:	;Interpreter routine.  
COMMENT ⊗  Used in body of c-m.  Starts uncritical section.  ⊗

	SETPRI #1	;Set the priority to 1
	TST (SP)+	;Flush old priority
	CCC		;Clear condition code
	RTS PC		;Done

;  CMDONE, CMDUR, CMFORCE, CMSENSE, COMPLY, CMPOFF, VMKFRC, TFRCST

CMDONE:	;Interpreter routine
COMMENT ⊗ Ends duration, force sensing & hardware monitor c-m's. Checks if
c-m was re-enabled while running and if so it will interpret the c-m's checker
(and so doing the c-m will be re-queued). Then it dismisses. ⊗
	MOV CMCB(R4),R0		;Get c-m control block
	BIC #CMRUN,CMSTAT(R0)	;Clear run bit
	BIT #CMDEST,CMSTAT(R0)	;Destroy ourself
	BEQ 1$
	JSR PC,RLFREE		;Yup - reclaim CMCB
	JMP TERMINATE		;Use interpreter terminate routine
1$:	BIT #CMENB,CMSTAT(R0)	;See if we were re-enabled
	BEQ 2$			;Nope - go away
	MOV CMSTRT(R0),IPC(R4)	;Reset IPC to LOC[c-m's checker]
	MOV RF,-(SP)		;Save RF
	MOV SP,RF		;RF ← LOC[Stack]
	JSR PC,INTERP		;Re-queue it
2$:	MOV PDB(R4),R0		;R0 ← LOC[c-m's PDB]
	MOV R3,PDBR3(R0)	;Make sure stack is okay
	MOV PDBPC(R0),PDBR2(R0)	;Save new PC(if any) in R2 since DISMIS kills it
	DISMIS			;Bye-bye
	JMP (R2)		;If return here use R2 to get where we should be

CMDUR:	;Interpreter routine
COMMENT ⊗ Schedules c-m body to be executed in time seconds. (The time is
on the stack.) Then returns control using RF. ⊗
	LDF @(R3)+,AC0		;Get time to wait in seconds
	MULF THOUS,AC0		;Convert it to milliseconds
	STCFI AC0,R0		; & make it integer
	SCHEDU PDB(R4),#1$,#USRDM,R0	;Schedule the c-m body to start later
	MOV RF,SP		;Restore stack
	MOV -2(SP),RF		;RF ← old PC
	RTS RF			;Just return

1$:	MOV CMCB(R4),R0		;R0 ← LOC[c-m's control block]
	BIT #CMENB,CMSTAT(R0)	;See if we're still enabled
	BNE 2$
	DISMIS			;If not then go away
2$:	JMP CMGO		;Set flags & go interpret the c-m's body

VMKFRC:	;Interpreter routine
COMMENT ⊗ Takes force vector (on R3 stack) and makes it into a frame with the x-axis
along the force vector. Always followed by a call to TFRCST which calls SETC. ⊗
	JSR PC,UNITV		;Make it a unit vector
	JSR PC,NOCMP		;Don't compact for a while
	MOV (R3)+,R0		;R0 ← LOC[unit vector]
	LDF (R0)+,AC0		;Get X
	LDF (R0)+,AC1		;Get Y
	LDF (R0)+,AC2		;Get Z
	STF AC0,AC4		;Copy X
	STF AC1,AC5		;Copy Y
	JSR PC,GETTRN		;R0 ← -(R3) ← LOC[new trans]
	STF AC0,(R0)+
	STF AC1,(R0)+		;Fill in 1st column with unit vector
	STF AC2,(R0)+
	MULF AC0,AC0		;X↑2
	MULF AC1,AC1		;Y↑2
	ADDF AC1,AC0		;X↑2 + Y↑2
	CFCC			;Check if X = Y = 0
	BNE 1$			; & if not skip ahead
	CLRF (R0)+
	STF AC2,(R0)+		;Next column is (0 Z 0)
	NEGF AC2
	CLRF (R0)+
	STF AC2,(R0)		;Last column is (-Z 0 0)
	BR 2$			;Jump ahead
1$:	JSR PC,@LSQRTF		;AC0 ← SQRT(X↑2 + Y↑2)
	LDF AC4,AC3		;Get X
	LDF AC5,AC1		;Get Y
	NEGF AC1		;Negate Y
	DIVF AC0,AC1		;a = -Y / SQRT(X↑2 + Y↑2)
	DIVF AC0,AC3		;b =  X / SQRT(X↑2 + Y↑2)
	STF AC1,(R0)+
	STF AC3,(R0)+		;Fill in 2nd column with (a b 0)
	CLRF (R0)+
	STF AC2,AC0		;Copy Z
	MULF AC1,AC2		;aZ
	MULF AC3,AC0		;bZ
	NEGF AC0		;-bZ
	MULF AC4,AC3		;bX
	MULF AC5,AC1		;aY
	SUBF AC1,AC3		;bX - aY
	STF AC0,(R0)+
	STF AC2,(R0)+		;Fill in 3rd column with(-bZ,aZ,bX-aY)
	STF AC3,(R0)+		;  it's the cross product of the other 2 columns
2$:	JSR PC,YESCMP		;OK to compact again
	CCC
	RTS PC			;Done - return

TFRCST:	;Interpreter routine
COMMENT ⊗ Gets force frame off of the R3 stack, arm & co-ordinate system bits follow
via the IPC. Calls SETC. ⊗
	JSR PC,GETARM		;See which arm we're using → R0
FRCST0:				; Entry point for POINTY
	FETCH R1		;Get bits for SETC: coord system (hand/table)
	BIS R1,R0		;R0 ← coord sys + arm
	MOV (R3)+,R1		;R1 ← LOC[force coordinate matrix]
	JSR PC,@LSETC		;Initialize the force system
	TST R0
	BEQ 1$
	ALERR CMNSET		;Complain if any problems
1$:	CCC
	RTS PC			;Done - return

CMFORCE: ;Interpreter routine
COMMENT ⊗ Gets force value (scalar on R3 stack) and queues c-m on force signal list.
Then returns control using RF. ⊗
	JSR PC,GETARM		;See which arm we're using → R0
CMFRC0:				; POINTY entry point
	LDF @(R3)+,AC0		;Get the force threshold value
	MOV PDB(R4),R1		;R1 ← LOC[c-m's PDB]
	MOV CMCB(R4),R2		;R2 ← LOC[c-m's control block]
	BIS CMBITS(R2),R0	;R0 ← c-m's force bits + arm
	MOV #CMGO,R2		;R2 ← when triggered start below
	JSR PC,@LFRCSIG		;Put the c-m in the force signal list
	TST R0
	BEQ 1$
	ALERR CMNFRC		;Complain if any problems
1$:	MOV RF,SP		;Restore stack
	MOV -2(SP),RF		;RF ← old PC
	RTS RF			;Just return

CMGO:	MOV PDB(R4),R0		;R0 ← LOC[c-m's PDB]
	MOV USKMAX(R0),SP	;Reset stack pointer
	MOV CMCB(R4),R0		;R0 ← LOC[c-m's control block]
	BIT #CMENB,CMSTAT(R0)	;Check that we are still enabled
	BNE 2$			; Yup - skip ahead
	JMP CMDONE		; Nope - so go away
2$:	BIS #CMRUN,CMSTAT(R0)	;Set the run bit
	BIC #CMENB,CMSTAT(R0)	;Clear the enable bit
	JMP INTERP		;Go interpret the c-m's body

COMPLY: ;Interpreter routine
COMMENT ⊗ Gets magnitude of force to apply (scalar on R3 stack) and the control bits via
(the arm and force component to apply) follow via the IPC. ⊗
	JSR PC,GETARM		;See which arm we're using → R0
CMPLY0:				;Entry point for POINTY
	FETCH R1		;Get bits for COMPLY
	BIS R1,R0		;R0 ← control bits + which arm
	LDF @(R3)+,AC0		;Get the force value
	JSR PC,@LBISON		;Set up the force to apply
	TST R0
	BEQ 1$
	ALERR CMNCMP		;Complain if any problems
1$:	CCC
	RTS PC			;Done - return

CMPOFF: ;Interpreter routine
	ALERR NOCMPF		;Complain - CMPOFF hasn't been written yet
	CCC
	RTS PC

CMSENSE: ALERR CMNOSE		;Aren't any of these guys yet
	MOV RF,SP		;Restore stack
	MOV -2(SP),RF		;RF ← old PC
	RTS RF			;Just return

DATA
CMNSET::ASCIE </COULDN'T INITIALIZE FORCE SYSTEM/>
CMNFRC::ASCIE </COULDN'T QUEUE FORCE CMON/>
CMNCMP::ASCIE </COULDN'T SET UP FORCE COMPLIANCE/>
NOCMPF::ASCIE </CAN'T TURN OFF COMPLIANCE YET/>
CMNOSE::ASCIE </HARDWARE MONITORING ISN'T READY YET/>
CODE
;Events:  SIGNAL, WAITE, PAUSE

COMMENT ⊗ Events can be created (at the beginnings of blocks is the
usual place), signaled, awaited (in the middle of a block) and
destroyed (at the end of a block).  Each event is a variable, that
is, it is refered to by a level-offset pair.  However, its place in
the environment does not point to a graph node, since there is no
such thing as attachment to an event.  The event itself is stored in
the environment.  The garbage collector marking phase had better
understand this.  ⊗

SIGNAL:	;Interpreter routine.  Signal the event of the level-offset pair.
	FETCH R0	;R0 ← level-offset pair.
SIGNL0::		; entry point for POINTY
	JSR PC,GETARG	;R0 ← equivalent pointer into environment
	EVSIG 2(R0)	;Signal that event.
	CCC		;Clear condition code.
	RTS PC		;Done

WAITE:	;Interpreter routine.  Wait on the event of the level-offset pair.
	FETCH R0	;R0 ← level-offset pair.
WAITE0:			; entry point for POINTY
	JSR PC,GETARG	;R0 ← equivalent pointer into environment
	EVWAIT 2(R0)	;Wait on that event.
	BCC 1$		;Return OK?
	JMP TERMINATE	;The event was destroyed.  I guess we should depart cleanly.
1$:	JSR PC,NOTICE	;Assume the world has gone awry.
	CCC		;Clear condition code.
	RTS PC		;Done

PAUSE:	;Interpreter routine
COMMENT ⊗ Pause n seconds, where n is on the stack.  ⊗
	LDF @(R3)+,AC0	;AC0 ← wait time
	MULF THOUS,AC0	;AC0 ← time, in milliseconds
1$:	CMPF MAXWT,AC0	;Check that we don't try to sleep for more than 30 sec
	CFCC		; or the kernel won't like us
	BPL 2$		;Skip ahead if less than max allowable
	SUBF MAXWT,AC0	;Update remaining time to pause
	SLEEP #30000.	;Sleep for half a minute
	BR 1$		; & wait the rest of the time
2$:	STCFI AC0,R0	;R0 ← time in milliseconds
	BEQ 3$		;Don't bother if time = 0
	SLEEP R0	;The pause that refreshes
3$:	CCC		;Clear Condition code
	RTS PC		;Done

DATA
THOUS:	.FLT2 1000.0
MAXWT:	.FLT2 30000.0
CODE
;Input routines:  PROMPT, QUERY, SCALRD

PROMPT:	;Interpreter routine
	EVWAIT CSLEVT	;Grab the console
1$:	MOV #3$,R0	;Say we want a "P" to proceed
	JSR PC,TYPSTR
	JSR PC,INCHR	;R1 ← reply char
	MOV R1,R0
	JSR PC,TYPCHR	;Echo it
	BIC #40,R1	;Make it upper case.
	CMP #'P,R1	;A valid response?
	BEQ 2$		; yup - proceed
	BR 1$		;Go and ask again
2$:	MOV #CRLFX,R0	;Type a crlf
	JSR PC,TYPSTR
	EVSIG CSLEVT	;Release the console
	JSR PC,NOTICE	;Since arm may have been moved
	CCC
	RTS PC

DATA
3$:	.BYTE 15, 12		;crlf
	.ASCIZ /Type P to proceed:  /
CODE

QUERY:	;Interpreter routine
	EVWAIT CSLEVT	;Grab the console
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar]
1$:	MOV #5$,R0	;Say we want a boolean
	JSR PC,TYPSTR
	JSR PC,INCHR	;R1 ← reply char
	MOV R1,R0
	JSR PC,TYPCHR	;Echo it
	BIC #40,R1	;Make it upper case.
	CMP #'Y,R1	;A yes response?
	BEQ 2$		; yup - put true on stack
	CMP #'N,R1	;A no response?
	BEQ 3$		; yup - all done 
	BR 1$		;Go and ask again
2$:	MOV ONE,@(R3)
3$:	MOV #CRLFX,R0	;Type a crlf
	JSR PC,TYPSTR
	EVSIG CSLEVT	;Release the console
	JSR PC,NOTICE	;Since arm may have been moved
	CCC
	RTS PC

DATA
5$:	.BYTE 15, 12		;crlf
	.ASCIZ /Type Y or N: /
CODE

SCALRD:	;Interpreter routine
	EVWAIT CSLEVT	;Grab the console
	JSR PC,GETSCA	;R0 ← -(R3) ← LOC[new scalar]
1$:	MOV #2$,R0	;Say we want a scalar
	JSR PC,TYPSTR
	MOV #INBUF,R0	;Read a new line.
	JSR PC,INSTR
	MOV #INBUF,R0
	JSR PC,RELSCN	;AC0 ← number typed in
	TST R1		;Got anything?
	BNE 1$		;  nope - try again
	STF AC0,@(R3)	;Put number in desired place.
	EVSIG CSLEVT	;Release the console
	JSR PC,NOTICE	;Since arm may have been moved
	CCC
	RTS PC

DATA
2$:	.BYTE 15, 12	;crlf
	.ASCIZ /SCALAR, PLEASE: /
CODE	

;Output routines:  PRINT, VALPRN, VARPRN, TACKVAL, TYPVAL, CVFX

PRINT:	;Interpreter routine
	FETCH R0	;R0 ← Address of string
PRINT0:	EVWAIT CSLEVT	;label used by POINTY
	JSR PC,TYPSTR	;Type it out
	EVSIG CSLEVT
	CCC		;Clear condition code
	RTS PC		;Done

VARPRN:	
COMMENT ⊗ Interpreter routine.  Prints the graph node pointed to by
the level-offset of the argument.  ⊗
	JSR PC,GTVAL	;Let GTVAL put value on stack
	JMP VALPRN	;And let VALPRN take it from there.

VALPRN:	
COMMENT ⊗ Interpreter routine.  Prints the value the top of the stack
and pops it.  ⊗
	MOV (R3)+,R0	;R0 ← LOC[value cell]
	JSR PC,TYPVAL	;Go print it.
	CCC		;Clear condition codes
	RTS PC		;And return

TYPVAL:
COMMENT ⊗ R0 points to a value cell.  Prints it according to its
type.  Requires the floating package.  ⊗
	PUSH R3
	EVWAIT CSLEVT
	JSR PC,TYPVL
	EVSIG CSLEVT
	POP R3
	RTS PC

	;R0 = LOC[value cell], R1 = LOC[string] in some cases.
	;R2, R3 are available for use.

TYPVL:	MOV R0,R2		;R2 ← LOC[value cell]
;	MOV #CRLFX,R0		;CRLF
;	JSR PC,TYPSTR
	CMPB #SCLID,TAGID(R2)	;A scalar?
	BEQ 1$
	CMPB #VCTID,TAGID(R2)	;A vector?
	BEQ 4$
	CMPB #TRNID,TAGID(R2)	;A trans?
	BEQ 5$
1$:
;	MOV #SNAME,R0
;	JSR PC,TYPSTR		;"SCALAR "
	MOV #OUTBUF,R0
2$:	LDF (R2),AC0
	JSR PC,CVFX
	MOV #OUTBUF,R0
	JSR PC,TYPSTR
3$:
;	MOV #CRLFX,R0		;CRLF
;	JSR PC,TYPSTR
	RTS PC			;Done
4$:
;	MOV #VNAME,R0
;	JSR PC,TYPSTR		;"VECTOR "
	MOV #OUTBUF,R0
	LDF (R2)+,AC0
	JSR PC,CVFX
	LDF (R2)+,AC0
	JSR PC,CVFX
	BR  2$			;Bum code for last field.
5$:
;	MOV #TNAME,R0
;	JSR PC,TYPSTR		;"TRANS "
	MOV #3,R3		;R3 ← Number of rows
6$:	MOV #CRLFX,R0
	JSR PC,TYPSTR
	MOV #OUTBUF,R0
	LDF (R2),AC0
	JSR PC,CVFX
	LDF 14(R2),AC0
	JSR PC,CVFX
	LDF 30(R2),AC0
	JSR PC,CVFX
	LDF 44(R2),AC0
	JSR PC,CVFX
	MOV #OUTBUF,R0
	JSR PC,TYPSTR
	ADD #4,R2		;Next row
	SOB R3,6$
	MOV #CRLFX,R0
	JSR PC,TYPSTR
	MOV #OUTBUF,R0
	MOV #3,R3		;Now do the 0 0 0 1 row
7$:	CLRF AC0
	JSR PC,CVFX
	SOB R3,7$
	LDF ONE,AC0
	JSR PC,CVFX
	MOV #OUTBUF,R0
	JSR PC,TYPSTR
	MOV #CRLFX,R0
	JSR PC,TYPSTR
	BR  3$			;Go to the exit stage

CVFX:	;Version of CVF that saves R1.
	PUSH <R1>
	JSR PC,CVF
	POP <R1>
	RTS PC

;DATA
;SNAME::	.ASCIZ /SCALAR /
;VNAME::	.ASCIZ /VECTOR /
;TNAME::	.ASCIZ /TRANS /
;CODE
;  BREAK, NOOP, TOPAL

BREAK:	;Interpreter routine
	MOV #BRKMES,R0
	JSR PC,TYPSTR
	BPT		;Cause a DDT break
	CCC		;Clear condition code
	RTS PC		;Done
DATA
BRKMES:: ASCIE </
PROGRAM BREAK/>
CODE

TOPAL:	;Interpreter routine
        COMMENT ⊗ Escape to PAL.  JSRs to the pseudo code.  That code
        should return via: 
            MOV PC,R0
            RTS PC
	⊗
	JSR PC,@IPC(R4)	;Fly
	ADD #2,R0	;R0 ← Proper new IPC
	MOV R0,IPC(R4)	;Hope R4, R3 still OK!
	RTS PC		;Done.

;Initialization ops:  PROG, ENDP, FIXIT

PROG:
COMMENT ⊗  Zeros the value & calc fields for the variables in the system
environment initializing it & makes the main interpreter environment.  ⊗
	JSR PC,NEWENV		;Create the main environment
	MOV R0,ENV(R4)		;Store away pointer to the environment
	MOV #1,LEV(R4)		;Establish the starting lexical level
	MOV #SYSENV,SLINK(R0)	;Set up the pointer to SYSENV
	MOV #SYSENV+4,R1	;R1 ← first entry in system environment
1$:	BIT #HDRTYP,(R1)+	;Check access mechanism - only header or direct
	BEQ 3$			;Handle direct accesses below
	MOV (R1)+,R0		;R0 ← LOC[header]
	BIT #FTYPE,TYPE(R0)	;See if device
	BEQ 2$			;Don't zap value or invmrk fields for devices
	MOV #1,INVMRK(R0)	;Now invalid
	CLR VAL(R0)		;Zero old value
2$:	CLR CALCS(R0)		;Kill any old calcs
	BR 4$
3$:	CLR (R1)+		;Zero old value
4$:	CMP #SYSEND,R1		;Any more to init?
	BHI 1$			;Go do them
	MOV #TWO,SPDFAC+2	;Initialize speed-factor to 2.0
	CCC			;Clear condition code
	RTS PC			;Done

ENDP:
COMMENT ⊗ Releases main interpreter environment. ⊗
	JMP TERMINATE		;Done with the interpreter

FIXIT:	
COMMENT ⊗ This should only have to be called from DDT.  Unwedges the
servos.  ⊗
	MOV #34,R0		;
	JSR PC,GTFREE		;Get a device block
	MOV R0,-(SP)		;
	MOV R0,R1		;
	JSR PC,@LINTARM		;Initialize all servos
	TST R0			;All well?
	BEQ 1$			;Yes
	MOV R0,-(SP)		;No
	MOV #FIXM,R0		;Complain.
	JSR PC,TYPSTR		;   without getting back into DDT prematurely
;	MOV (SP)+,R0		;
;	JSR PC,TYPOCT		;
	mov (sp)+,r0		; print corresponding error message
	mov armsg(r0),r0
	jsr pc,typstr
1$:	MOV (SP)+,R0		;
	JSR PC,RLFREE		;Reclaim the device block
	RTS PC			;
DATA
FIXM::	ASCIE </
CAN'T INITIALIZE ARM./>
ARMSG:	arm1
	arm2
	arm3
	arm4
	arm5
	arm6
	arm7
	arm10
	arm11
	arm12
	arm13
	arm14
	arm15
arm1:	ascie </
Could not attach to requested joint(s)/>
arm2:	ascie </
Incorrect number of joints requested to be driven/>
arm3:	ascie </
Wipers could not be read within their operating range/>
arm4:	ascie </
Arm solution does not exist/>
arm5:	ascie </
Unknown touch sensor requested/>
arm6:	ascie </
No more free slots in touch sensor event list/>
arm7:	ascie </
Arm interface power supply turnd off/>
arm10:	ascie </
Reference power supply out of range/>
arm11:	ascie </
Zero velocity tachometer reading out of range/>
arm12:	ascie </
Attempted to switch arms while force servoing/>
arm13:	ascie </
No more free slots in force sensor event list/>
arm14:	ascie </
Need all 6 arm joints in order to do force sensing/compliance/>
arm15:	ascie </
Can't force servo motion without polynomial/>

CODE
;BUGS

COMMENT ⊗
No way to kill enabled event cmons. Need to add a kernel call that removes
a given pdb from a given event wait list. Ditto for duration cmons.
⊗